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1  Abstract 


Semi-analytic  methods  to  generate  minimum  and  near-minimum  time  velocity  profiles  for  a  vehicle 
along  a  specified  path  are  presented  in  this  report. 

Initially  we  adopt  a  point  mass  parametrization  of  the  vehicle  with  specified  acceleration  limits. 
In  generating  the  optimal  velocity  profile,  several  undesirable  cases,  where  loss  of  controllability 
occurs,  and  which  have  been  neglected  in  the  literature,  are  dealt  with  in  this  work.  A  receding 
horizon  implementation  is  also  proposed  for  the  on-line  implementation  of  the  velocity  optimizer. 
Robustness  of  the  receding  horizon  algorithm  is  guaranteed  by  the  use  of  an  adaptive  scheme  that 
determines  the  planning  and  execution  horizons.  Application  to  a  Formula  1  (FI)  circuit  with  a 
comparison  between  the  infinite  and  finite  receding  horizon  schemes  provides  a  validation  of  the 
proposed  methodology. 

Extensions  of  the  point  mass  methodology  to  a  half-car  model  are  presented  next  in  order  to 
recover  the  missing  attitude  (yaw)  information.  The  acceleration  limits  (GG-diagram)  of  the  half¬ 
car  model  is  determined  by  the  available  tire  friction  forces  in  the  front  and  rear  axles.  We  present 
three  extensions  of  the  point  mass  methodology  to  the  half-car  model.  In  the  first  extension  we 
directly  implement  the  optimal  control  strategy  of  the  point  mass  case  to  the  half-car  model.  In  the 
second  extension  the  optimal  control  strategy  of  the  point  mass  case  is  interrupted  by  a  stabilizing 
control  logic  when  the  vehicle  slip  angle  increases  beyond  a  certain  value  and  the  yaw  dynamics  tend 
to  instability.  Finally,  in  the  third  approach  we  enforce  the  additional  constraint  that  the  vehicle 
tracks  the  path  with  zero  slip  angle  and  determine  the  acceptable  acceleration  limits  subject  to  the 
new  constraint. 
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2  Statement  of  the  Problem 


Achieving  autonomous  operation  in  open  terrain  remains  a  challenging  problem  in  the  development 
of  land  vehicles.  This  is  due  to  the  uncertainty  of  the  environment  the  vehicle  operates  in,  as  well  as 
due  to  the  poor  characterization  of  the  complex  vehicle  dynamics  and  their  integration  with  sensors 
and  actuators.  A  solution  to  this  problem  requires  not  only  sophisticated  hardware  components  (for 
actuation,  sensing,  and  communication),  but  also  advanced  software  algorithms  and  supervisory 
control  strategies  that  can  make  use  of  the  full  capability  of  these  components. 

A  class  of  vehicles  we  envision  to  be  completely  automated  in  the  future  are  ground  wheeled 
vehicles  (Fig.  1(a))  that  operate  in  hostile  off-road  environments  (e.g.,  battlefields).  A  typical 
mission  would  be  to  drive  the  vehicle  from  point  A  to  point  B,  avoid  any  obstacles,  while  minimizing 
the  exposure  to  danger;  see  Fig.  1(b).  In  general,  minimization  of  the  exposure  to  danger  involves 
driving  through  a  trajectory  in  minimum  time  or  maximum  average  velocity. 


B 

X 


(a)  (b) 


Figure  1:  (a)  autonomous  military  off-road  vehicles  will  exhibit  a  high  degree  of  tactical  mobility,  while 
eliminating  the  risk  of  loss  of  human  lives;  (b)  a  typical  mission  scenario  will  involve  an  autonomous  vehicle 
entering  a  hazardous  area  to  perform  its  mission  objective,  while  avoiding  obstacles  and/or  minimizing  its 
exposure  to  enemy  threats  and  countermeasures. 

The  objective  of  this  research  project  is  to  built  on  our  previous  experience  on  the  analysis 
of  the  dynamics  and  the  optimal  control  of  high-speed  wheeled  vehicles  in  order  to  develop  new, 
advanced  control  methodologies  for  the  on-line  control  of  autonomous  land  vehicles  that  mimic 
the  way  expert  humans  drive.  The  focus  of  this  work  will  be  in  autonomous  path  generation, 
navigation  and  tracking  for  unmanned  wheeled  terrestrial  vehicles  operating  in  high-speed.  The 
emphasis  on  high-speed  operation  stems  from  the  need  to  minimize  reaction  time  and  exposure  to 
external  threats.  Specifically,  the  following  problems  will  be  addressed  during  this  project: 

2.1  Challenges 

The  problem  of  trajectory  planning  for  high-speed  autonomous  vehicles  is  typically  dealt  with  in 
the  literature  by  means  of  numerical  optimization.  Several  published  results  have  addressed  path 
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planning  of  high-speed  ground  vehicles  [1,  2,  3,  4],  These  results  demonstrate  that  numerical  tech¬ 
niques  allow  one  to  incorporate  accurate,  high  order  dynamical  models  in  the  optimization  process, 
thus  producing  realistic  results.  In  fact,  the  optimal  solutions  generated  using  these  optimizers  are 
comparable  to  experimental  results  obtained  from  expert  race  drivers  [2,  3,  4].  On  the  other  hand, 
these  numerical  optimization  approaches  are  computationally  intensive,  and  they  cannot  be  readily 
applied  in  cases  where  the  environment  changes  unpredictably.  As  a  result,  they  are  not  suitable 
for  real-time  path-planning  optimization. 

In  the  work  of  Spenko  [5]  real-time  trajectory  planning  for  hazard  avoidance  of  high-speed 
Unmanned  Ground  Vehicles  (UGV’s)  in  rough  terrain  has  been  addressed.  A  fairly  rich  vehicle 
model  has  been  used  to  predict  and  avoid  roll-over  and  excessive  side-slip.  The  computational 
cost  is  mitigated  somewhat  by  choosing  the  hazard  avoidance  maneuver  from  a  library  of  off-line 
pre-computed  candidates. 

2.2  Approach 

An  alternative  approach  for  on-line  trajectory  optimization  of  ground  vehicles  is  proposed  in  this 
work.  Having  in  mind  the  requirement  for  reduction  of  the  computational  cost,  we  are  motivated  to 
explore  the  possibility  of  solving  the  trajectory  planning  problem  (or  at  least  part  of  this  problem) 
analytically  or  semi-analytically.  We  separate  the  geometric  problem  of  designing  the  optimal  path 
from  the  dynamic  problem  of  optimally  following  this  path  given  the  vehicle  dynamic  characteristics. 
Specifically,  we  assume  that  the  geometric  characteristics  of  the  reference  trajectory  are  provided 
a  priori.  This  means  that  the  path  to  be  followed  will  be  the  result  of  another  optimization 
step,  which  will  typically  incorporate  additional  constraints,  such  as  minimum  distance  traveled, 
minimum  average  curvature,  or  a  combination  of  the  two.  Since  the  dynamics  of  the  vehicle  are 
not  directly  included  in  this  optimization  step,  we  expect  a  reduced  computational  cost.  This 
separation  of  the  geometric  from  the  dynamic  problem  has  also  been  proposed  in  [6,  7]  and  [8].  The 
path  in  these  references  is  designed  using  geometric  principles  and  an  “intuitively  optimal”  velocity 
profile  is  generated  using  a  semi-analytical  approach,  by  taking  into  consideration  the  maximum 
acceleration  available  to  the  vehicle  at  each  point  on  the  path.  Notice  that  in  [6,  7,  8]  the  yaw 
dynamics  of  the  vehicle  are  neglected.  A  similar  problem  to  the  one  investigated  in  this  paper  has 
been  addressed  in  [9],  [10]  and  [11].  Therein  the  authors  investigated  the  minimum  time  solution 
for  a  robotic  manipulator  moving  its  tip  along  a  prescribed  path,  while  taking  into  consideration 
the  actuator  limits.  In  order  for  all  actuators  to  maintain  enough  control  authority  for  the  tip  to 
track  the  desired  path  exactly ,  a  state  constraint  is  introduced  which  defines  a  set  of  admissible 
velocities.  The  proposed  solution  consists  only  of  bang-bang  control  intervals,  and  the  velocity 
profile  may  coincide  with  the  allowable  limit  only  instantaneously.  Proof  of  optimality  is  provided 
by  point-wise  maximization  of  the  velocity.  The  extension  from  point-mass  to  a  half-car  model 
(that  includes  rotation  of  the  vehicle  about  its  vertical  axis)  creates  new  problems.  Specifically, 
direct  implementation  of  the  optimal  strategy  from  the  point-mass  model  may  lead  to  unstable 
oversteer  in  tight  corners.  We  propose  a  stabilizing  strategy  to  remedy  the  instability  without 
sacrificing  performance,  in  terms  of  speed. 
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3  Summary  of  Results 


3.1  Introduction 

In  this  work  we  first  concentrate  on  providing  a  rigorous  proof  of  optimality  of  the  approach  in 
[6,  7,  8]  for  a  point  mass  consideration  of  the  vehicle  (neglecting  the  yaw  dynamics)  using  optimal 
control  theory.  We  also  account  for  the  loss  of  controllability  due  to  limited  accelerating/braking 
and  cornering  forces.  The  set  of  admissible  velocities  is  explicitly  expressed  in  terms  of  the  accel¬ 
eration  capacity  of  the  vehicle  and  the  curvature  of  the  path  to  be  followed.  We  present  a  solution 
that  allows  the  velocity  to  stay  within  the  allowable  limit  using  alternative  control  strategies,  in 
conjunction  to  bang-bang  intervals,  as  necessary.  In  addition,  a  constructive  proof  of  optimality  for 
several  special  cases  of  paths  to  be  followed  is  provided.  The  necessary  optimality  conditions  are 
explicitly  derived,  which  allow  us  to  draw  conclusions  on  the  number  and  type  of  control  switchings. 
In  particular,  the  switching  points  are  found  semi-analytically  instead  of  numerically  (as  it  was  done 
in  [9],  [10],  [11]).  Application  to  an  FI  circuit  provides  a  validation  of  the  proposed  methodology. 

In  order  for  a  trajectory  optimization  scheme  to  be  suitable  for  on-line  implementation,  it  must 
be  able  to  adapt  to  changing  environments,  in  addition  to  having  a  reduced  computational  cost.  In 
[12,  13,  14]  real-time  trajectory  planning  for  autonomous  (aerial)  vehicles  using  a  receding  horizon 
scheme  was  proposed.  In  a  receding  horizon  scheme  the  optimization  is  not  performed  throughout 
the  whole  trajectory,  but  it  is  rather  computed  from  the  current  position  up  to  a  pre-specified  hori¬ 
zon.  The  vehicle  executes  part  of  the  computed  optimal  trajectory,  while  simultaneously  optimizing 
the  path  up  to  a  new  horizon.  The  search  space  at  each  optimization  step  is  considerably  reduced, 
which  results  in  reduced  computational  cost.  In  addition,  the  receding  horizon  implementation 
accounts  for  changes  in  the  environment  outside  the  optimization  horizon  for  each  step. 

We  propose  a  receding  horizon  implementation  of  the  previous  semi-analytical  optimal  velocity 
profile  algorithm.  The  algorithm  ensures  that  at  the  end  of  each  executed  subarc  the  vehicle  can 
reach  a  “safe  state”  (for  example,  complete  stop)  regardless  of  the  (a  priori  unknown)  changes  in 
the  environment  outside  the  planning  horizon.  This  is  achieved  by  designing  a  dynamic  scheme  that 
determines  appropriate  planning  and  execution  horizons.  Finally,  we  apply  the  receding  horizon 
optimization  scheme  to  an  FI  circuit  to  validate  our  approach. 

Afterwards  the  point  mass  methodology  for  minimum  time  velocity  profiles  is  extended  to  a 
vehicle  model  including  the  yaw  dynamics.  A  half-car  model  with  nonlinear  tire  friction  character¬ 
istics  is  introduced  and  the  acceleration  envelope  (GG-diagram)  of  the  vehicle  is  calculated  for  any 
operating  condition.  The  maximum  available  acceleration  and  maximum  available  deceleration  is 
determined  within  the  GG-diagram  and  a  direct  extension  of  the  optimal  control  strategy  of  the 
point  mass  case  is  proposed  using  the  dynamics  of  the  half-car  model.  Numerical  simulations  reveal 
that  the  stability  of  the  yaw  dynamics  needs  to  be  taken  into  consideration. 

Stability  of  the  yaw  dynamics  of  passenger  vehicles  has  been  a  subject  of  intense  research  in  the 
automotive  community  and  has  led  to  the  development  of  commercial  active  safety  systems  such 
as  the  Electronic  Stability  Program  (ESP).  The  ESP  uses  individual  wheel  braking  to  generate 
stabilizing  yaw  moments  in  critical  cases  where  the  vehicle  operates  close  to  an  estimated  stability 
margin.  In  [15]  this  stability  margin  is  characterized  by  the  vehicle  slip  angle  /3.  In  [16]  the  stability 
margin  is  determined  empirically  in  the  /3-/3  plane  and  its  dependence  on  the  velocity  of  the  vehicle 
is  demonstrated. 

We  present  two  approaches  for  a  stable  implementation  of  the  control  strategy  for  the  point  mass 
case  to  a  half-car  model.  In  the  first  approach  we  design  stabilizing  control  schemes  that  intervene 
during  execution  of  the  optimal  maximum  acceleration/maximum  deceleration  action  when  the 
vehicle  slip  angle  increases  and  the  yaw  dynamics  tend  to  instability.  In  the  second  approach  we 
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redefine  the  maximum  acceleration  limits  of  the  vehicle  subject  to  the  additional  constraint  of  zero 
vehicle  slip  angle  throughout  the  path. 

3.2  The  Point  Mass  Vehicle  Case 
3.2.1  Problem  Formulation 

Consider  a  vehicle  of  mass  m  travelling  along  a  prescribed  path,  with  given  acceleration  limits 
and  fixed  initial  and  final  position  and  velocity.  We  seek  the  velocity  profile  along  the  path  for 
minimum  travel  time.  The  path  is  described  by  its  radius  r(s)  at  each  point,  which  is  given  as 
a  function  of  the  path  length  coordinate  s,  or  equivalently  by  the  curvature  k(s)  along  the  path 
(Fig.  2).  The  cartesian  coordinates  at  any  point  on  the  path  can  be  calculated  using  the  following 
standard  transformation 


Figure  2:  A  vehicle  of  mass  m  travels  along  the  prescribed  path  r(s)  with  given  maximum  acceler¬ 
ation  limits  in  minimum  time. 


1 


r(s)’ 

x(s)  =  /  cos0((r)d<7, 


'SO 


The  equations  of  motion  are  given  by 

d2s 

= /ti 


m 


fnr(s), 


(1) 

(2) 


where,  ft  is  the  tangential  component  of  the  force  along  the  path,  and  fn  is  the  normal  (centripetal) 
force  such  that  the  vehicle  tracks  the  prescribed  path.  The  force  acting  on  the  vehicle  is  limited 
within  the  ellipse 

(f)2+(^)2-1s1  <3) 

This  is  shown  in  Fig.  2,  where  /™ax  is  the  maximum  longitudinal  force  and  /™ax  is  the  maximum 
lateral  force.  We  assume  that  the  initial  and  final  vehicle  velocities  are  given,  and  satisfy 


ds 
d  t 


< 


t=to 


fT*r(so) 


ds 
d  t 


< 


frxr(Sf) 


t=tf 


m 


m 


(4) 


in  order  for  the  initial  and  final  cornering  forces  to  be  less  than  the  allowable  limit  /™ax  and  also 
in  order  for  some  accelerating/braking  force  ft  to  be  available  at  to-  Moreover,  it  will  be  assumed 
that  the  velocity  of  the  vehicle  is  always  greater  than  or  equal  to  zero,  that  is,  ds/df  >  0  for  all 
t  £  [to,tf].  Specifically,  the  vehicle  is  not  allowed  to  reverse  direction,  a  natural  assumption  for  a 
minimum-time  problem. 

From  now  on,  and  unless  stated  otherwise,  we  assume  uniform  acceleration  limits  /™ax  =  /™ax  = 
.Ft nav,  i.e.,  the  total  force  lies  within  a  circle  of  radius  Fmax. 

Consider  the  following  state  assignment  and  change  of  time  scale: 

d  s 

r  =  (3t ,  Z\  =  a/3s,  Z2  =  a—,  (5) 

with  a  =  \J m/FmB ^  and  (3  =  aFmax/m.  The  state-space  representation  of  the  system  may  then  be 
written  as 


zi 

Z2 


Z'2 , 
ft 


TZ 

1  max 


(6) 

(7) 


where  (  )  denotes  derivative  with  respect  to  r.  The  control  input  in  this  formulation  is  ft,  and  the 
maximum  overall  acceleration  limit  Fmax/m  translates  to  a  state-dependent  control  constraint  as 
follows 


ft 


72 

z2 


+  G(*  l) 


-  1  <  0, 


(8) 


where  R(zi)  =  r(z\/(a(3)). 

The  control  constraint  (8)  can  be  written  as 


ft 


Ft 


=  U  \  1  - 


R2(z  i) 


,  u  £  [—1,  +1]. 


In  terms  of  the  new  control  variable  u  the  dynamics  of  the  system  is  written  as 


zi  =  z2,  z2=u\  1- 


R2(z1) 


,  u  £  [—1,  +1]. 


(9) 


(10) 


In  terms  of  the  original  variables,  the  equation  of  motion,  using  the  elliptic  force  envelope  constraint 
(3)  can  be  written  as 


d2s 


m 


dt2 


m  //tmax\2  /ds\2 


(11) 


where  u  £  [—1,  +1]. 

Note  that  the  dynamics  (10)  are  well  defined  only  for  trajectories  inside  the  region  S  C  M2  of 
the  state  space  defined  by 


S  =  {(zi,z2)  ■  C0(z1,z2)  =  z\  -  |F(zi)|  <  0}. 


(12) 


In  addition,  controllability  is  maintained  only  at  the  interior  of  the  set  S.  At  the  boundary  of  the 
set  S  controllability  is  lost.  The  following  lemma  states  that  unless  we  have  a  path  of  constant 
curvature,  the  state  constraint  Cq(z\,  z2)  <  0  is  always  inactive  for  any  finite  interval  of  time, 
hence  controllability  is  maintained  for  any  path  of  nonzero  curvature.  In  the  following,  R! (z\)  = 
dR(zi)/dz\. 
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Lemma  1  Assume  R'(zi)  /  0  for  any  z\  E  ( za,zp )  C  [zio,zif],  where  z\o  =  z\(tq)  and  z\ f  = 
z\{jf).  Then  the  manifold  dS  =  {(zi,z2)  ■  Co(zi,z2)  =  0}  is  not  invariant  for  the  system  (10)  for 
any  control  u. 

Proof:  Invariance  of  dS  with  respect  to  (10)  implies  that 

—ZiR'(zi)  sgnR(zi)  +  2z2z2  =  0,  (13) 

equivalently, 

-z2R'{zi)  sgni?(zi)  =  0,  (14) 

since  z2  =  0  on  dS  for  any  u  E  [— 1,-fl].  Since  z2  >  0  for  all  z\  E  (210,21/),  the  last  equation  is 
satisfied  if  and  only  if  R'(z\)  =  0.  ■ 

An  immediate  consequence  of  Lemma  1  is  the  fact  that  dS  is  invariant  under  (10)  only  for  paths 
of  constant  curvature. 

Note  that  the  flow  of  the  trajectories  of  (10)  in  the  vicinity  of  the  constraint  Cq(zi,  z2)  =  0  are 
given  by 


dC0(z1,z2)  _  dC0.  dC0. 

dt  dz\  Zl  dz2  Z 2 

=  —ziR'(zi)  sgnR(zi)  +  2z2z2 
=  - z2R\zi )  sgnR(zi). 

It  follows  that 

d t  <  ^  sSn-^(zi)  >  0  (15) 

and 

dt  ’  >  ^  R'{zi)  sgnR(zi)  <  0.  (16) 

The  following  corollary  is  therefore  immediate. 


Corollary  1  The  set  S  is  invariant  for  the  system  (10)  only  for  paths  of  monotonically  decreasing 
curvature  (increasing  radius).  Such  paths  are  characterized  by  the  inequality  R’ (z\)  sgnR(z\)  >  0. 

Given  a  certain  path,  characterized  by  its  radius  R(z\),  the  velocity  z2  for  which  the  constraint 
Cq(z\,z2)  =  0  is  satisfied  is  thus  of  extreme  importance  for  our  problem.  We  will  denote  this 
velocity  by  z2crn(z\).  It  is  given  by 

Z2crit(2l)  -  V\R(zl)\-  (17) 

When  z2(zi)  =  ^2crit(~i)  for  some  z\  E  [zio,^i/]  loss  of  controllability  ensures.  Corollary  1  essen¬ 
tially  states  that  for  paths  of  monotonically  decreasing  curvature  loss  of  controllability  can  occur 
only  instantaneously.  This  can  also  be  seen  from  the  following  simple  argument.  Assume,  for 
instance,  that  at  some  point  rc  E  (to ,?/),  z2(tc)  =  z2cr\t(zi(Tc)) .  The  tangential  component  of 
the  acceleration  becomes  zero  and  z2(tc)  =  0.  Since  the  vehicle  travels  on  a  path  of  monoton¬ 
ically  decreasing  curvature  (increasing  radius),  |i?(^i(rjl_))|  >  |i?(2i(rc))|,  while  z2 (r+)  =  z2(rc). 
It  follows  that  the  square  root  in  the  rhs  of  equation  (10)  will  take  a  positive,  non-zero  value  at 
r  =  t+  and  the  system  will  regain  controllability.  For  a  path  of  monotonically  increasing  cur¬ 
vature  (decreasing  radius)  on  the  other  hand,  the  condition  z2(tc)  =  Z2cnt(zi(rc))  at  some  point 
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Tc  E  (r0,T/)  leads  to  Z2(tc)  =  0  and  2:2  (r+)  =  Z2(rc),  and  since  |-R(2i(t+))|  <  |ii(zi(rc))|,  it  follows 
that  2t|(t+)  >  |.R(zi(t+))|.  The  quantity  inside  the  square  root  at  the  right-hand-side  of  (10)  be¬ 
comes  negative  at  .  The  equations  are  infeasible  and  a  larger  centripetal  force  than  the  available 
one  Fmax  is  needed  for  the  vehicle  to  negotiate  the  path.  It  follows  that  for  a  path  of  monotonically 
increasing  curvature  (characterized  by  the  inequality  R!{z\)  sgnR(z\)  <  0)  we  cannot  allow  the 
vehicle  to  reach  Z2crit(-H).  In  Section  3.2.5  we  discuss  this  case  in  great  detail  and  we  show  that 
optimal  paths  necessarily  remain  in  S. 

We  know  turn  to  the  solution  of  the  minimum  time  problem  for  system  (10). 


3.2.2  Optimal  Control  Formulation 


In  reference  to  the  system  (10),  and  given  fixed  initial  conditions  z\o  =  z±,  Z20  =  Z2  at  r  =  tq  and 
final  condition  z\f  =  zi,  £2/  =  £2  at  r  =  r/,  we  desire  the  optimal  control  u  that  drives  the  system 
(10)  from  point  A  to  point  B  of  the  cartesian  plane  (Fig.  2)  in  minimum  time  tj  subject  to  (12). 
We  adopt  the  notation  z[  and  2^  for  the  path  length  coordinate  z\  and  velocity  Z2  of  a  point  P  of 
the  prescribed  path  R(z\).  Thus,  we  have  zf  =  z  10,  z^  =  Z20>  zf  =  z\f  and  =  22/.  Notice  that 
without  loss  of  generality  we  may  assume  that  22(1")  >0,  V  r  E  (to,  tj). 

The  cost  function  to  be  minimized  is  written  as 


J  = 


rrf 

da. 


(18) 


’TO 


The  Hamiltonian  for  this  problem  is 


H(z,  A,  u)  =  1  +  Aiz2  +  A 2u  J 1  -  ))  +  vCoizi,  Z2). 


The  system  of  adjoint  equations  is 


•  =  _dH  =  _x  4  ^(n) 

1  dz!  2U^l  -  zl/R^Zi)  R3(zi) 

■  OH  zi 

A2  —  — 77 —  —  —  Ai  +  2A2U - 

dZ2  R?(z1)y/l-4/R?(z1) 


+  n  sgnR(zi)R'(zi), 

-  2 z2n- 


The  Kuhn- Tucker  conditions  imply 

fi  =  0  for  Cq(zi,  Z2)  <  0  and  /i  >  0  for  Cq(zi,Z2)  =  0. 


(19) 

(20) 
(21) 

(22) 


The  transversality  condition  implies  H{jf)  =  0,  and  since  the  Hamiltonian  does  not  depend 
explicitly  on  time  it  also  follows  that 


H{t)  =  0,  VtE[t0,t/].  (23) 

Consider  first  the  case  of  an  inactive  constraint,  60(21,2:2)  <  0.  In  this  case  /1  =  0  and 
Pontryagin’s  Maximum  Principle  leads  to  the  optimal  control 

*  ■  tt/  \  \  1  f°r  A2  >  0,  /_  .. 

u  =  argmmue[_1)+1]iJ(z,  A,  u)  =  |+1  for  ^  <  Q  (24) 

which  implies, 


u*(t)  =  -sgnA  2(r). 
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(25) 


Therefore  A2  is  the  switching  function,  which  determines  the  value  of  u*. 

Let  us  consider  the  possibility  of  a  singular  control  interval  in  the  optimal  solution,  i.e.,  the 
existence  of  a  time  interval  (ti,T2)  C  [to ,  t/]  such  that  A2(t)  =  0,  for  all  r  G  (71,72).  Equation  (20) 
implies  that  Ai(r)  =  0  for  all  r  G  (n,  72),  or  equivalently  Ai(r)  =  A10  =  constant  for  all  r  G  (ri,  T2). 
Equation  (21)  implies  ^2(7")  =  —  Aior  for  all  r  G  (71,72).  In  addition,  A2(t)  =  0  and  A2(t)  =  0  for 
all  r  G  (ri,  t2),  and  thus  we  have  A10  =  0  and  Ai(r)  =  A2(t)  =  0  for  all  r  G  (iq,  T2).  Equation  (19) 
then  gives  H{t)  =  1  for  t  G  (ti,T2),  which  contradicts  the  condition  (23)  that  H(t)  =  0  for  all 
T  e  [T0,Tf\. 

We  have  thus  proven  the  following  proposition. 

Proposition  1  Assuming  that  throughout  the  optimal  trajectory  Co  (2:1,22)  <  0,  there  can  he  no 
singular  subarc. 

This  proposition  states  that  to  optimally  transverse  a  path  in  minimum  time,  the  maximum 
available  force  must  be  used  at  all  times  and  the  optimal  trajectory  is  composed  only  of  bang-bang 
subarcs  [u  =  +1  or  u  =  —1),  assuming  that  the  optimal  state  trajectory  remains  inside  S. 

3.2.3  Solution  for  Special  Cases  of  Path  Curvature:  Inactive  Constraint 

In  the  following,  we  provide  solutions  to  the  previous  minimum-time  problem  for  several  special 
cases  of  R(z±).  First,  we  consider  the  simplest  case  when  the  constraint  (12)  remains  inactive. 
According  to  Lemma  1  this  occurs  only  if  R'(z\)  7^  0.  We  distinguish  two  different  cases:  paths  of 
decreasing  curvature  and  paths  of  increasing  curvature. 

3.2.4  Path  of  Decreasing  Curvature 

Consider  a  path  of  monotonically  decreasing  curvature  from  point  A  to  point  B  in  the  cartesian 
plane  denoted  by 

VAB  =  {(zhR(zl))  :  R'{zi)sgnR{zi)  >0,  Z\  G  [zi,zf]}  .  (26) 

From  Proposition  1  we  know  that  the  optimal  path  is  composed  solely  of  subarcs  of  maximum 
acceleration  or  maximum  deceleration. 

Equation  (20)  with  R'{z\)  sgnf?(2i)  >  0  yields  Ai(r)  >  0,  for  all  r  G  [ro,r/].  Suppose  now  that 
there  exists  a  switching  time  t\  G  (to,t/).  It  follows  that  A2(ti)  =  0.  The  transversality  condition 
(23)  implies  Ai(ri)  =  —1/2:2(71).  For  any  r  G  [ro,ri)  we  have  that 

1  .  -l  +  |A2|y/l-4/^(Zl)  _ 

/  \  —  /  \  \  w 

Z2\t)  z2\t) 

<  Aifa)  = - (27) 

Z2  (ri) 

since  Ai  is  non-decreasing.  Inequality  (27)  implies  that  22 (r)  <  z2 (ri)  for  all  r  G  [ro,ri),  from 
which  we  conclude  that  n  is  a  switching  point  from  u  =  +1  to  u  =  —  1. 

Following  the  same  steps  as  for  the  switching  point  T\ ,  it  is  easy  to  prove  that  any  other 
switching  point  r2  G  (ro,ri)  has  to  be  from  u  =  +lto7i=— 1.  Obviously,  there  can  be  no 
consecutive  switching  points  from  u  =  +1  to  «  =  —  1  without  a  switching  from  u  =  —1  tou  =  -l-l 
in  between.  Thus,  we  rule  out  the  possibility  of  existence  of  a  second  switching  point  r2  G  (ro,ri). 
Finally,  suppose  that  there  exists  a  switching  point  73  G  (ti,t/).  The  transversality  condition 
(23)  implies  Ai(t3)  =  — 1/22(73).  Since  Ai  is  non-decreasing  we  have  that  Ai(ri)  <  Ai(r3)  or  that 
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—1/z2{t\)  <  —1/2:2(73)  and  finally  that  Z2{t\)  <  2:2(73),  implying  that  the  vehicle  accelerates  from 
t\  to  73,  which  contradicts  the  fact  that  u(t )  =  -1  for  r  £  (71,73).  It  follows  that  we  can  have 
only  one  switching  in  the  control. 

Let  now  Zj^(zi)  be  the  characteristic  constructed  by  forward  integration  of  (10)  from  (zj^z^1) 
with  u  =  +1,  and  Zjj(zi)  be  the  characteristic  constructed  by  backward  integration  of  (10)  from 
(zi  ,  Z2)  with  u  =  —1.  We  have  therefore  the  following  proposition  for  the  optimal  trajectory  on 
paths  of  monotonically  decreasing  curvature. 

Proposition  2  In  the  case  of  a  path  of  monotonically  decreasing  curvature  V\B  there  can  be  at 
most  one  switching  in  the  control,  from  u  =  +1  to  u  =  —  1.  In  this  case  the  optimal  solution  is 
given  by 

zl(zi)  =  m.m.{Z\{zx),ZB{z1)}  .  (28) 

It  is  easy  to  prove  the  optimality  of  (28)  directly,  by  showing  that  z|(zi)  in  (28)  maximizes  the 
velocity  pointwise  for  all  z\  E  [zj4,  zf3]. 


3.2.5  Path  of  Increasing  Curvature 

Consider  a  path  of  monotonically  increasing  curvature  from  point  A  to  point  B  denoted  in  the 
cartesian  plane  by 

VAB  =  {(zi,-R(zi))  :  R' (zi)  sgnR(zx)  <0,  Zi  E  [zf,zf]}  .  (29) 

Similar  to  the  case  of  a  path  of  decreasing  curvature,  the  optimal  path  is  composed  only  of  subarcs 
of  maximum  acceleration  or  maximum  deceleration,  assuming  that  the  optimal  trajectories  remain 
in  the  interior  of  S  (Proposition  1).  An  analysis  similar  to  the  one  of  Section  3.2.4  can  be  followed 
to  show  that,  assuming  the  trajectories  remain  in  S ,  there  can  be  at  most  one  switch  in  the  control, 
from  u  =  +1  to  u  =  —  1.  Below  we  show  that  the  optimal  trajectory  does  indeed  remain  in  S. 

As  before,  let  Zj^(zi)  be  the  characteristic  constructed  by  forward  integration  of  (10)  from 
( zf,z 2)  with  u  =  +1,  and  Zjj(zi)  be  the  characteristic  constructed  by  backward  integration  of 
(10)  from  (zf,z^)  with  u  =  —1. 

Lemma  2  Assuming  z.f  <  Z2Crit(zf ),  then  ZB(z\)  <  Z2crit(~i)  for  all  z \  E  [zj4,  zf). 


Proof:  The  proof  involves  two  steps.  First,  we  construct  the  locus  M.  of  points  in  the  Z1-Z2  plane 
having  the  following  property:  the  slope  of  any  trajectory  beginning  from  any  point  in  A4  C  S  using 
the  control  u  =  —  1  is  less  than  or  equal  to  the  slope  of  Z2Crit(-2:i)-  In  the  second  step  we  show  that 
for  characteristic  path  starting  in  S\ A4  constructed  by  backward  integration  of  (10)  with  u  =  — 1 
remains  in  S. 

To  this  end,  note  that  the  smallest  possible  slope  in  the  Z1-Z2  plane  of  any  feasible  trajectory  is 
achieved  using  maximum  deceleration  (u  =  —1).  In  particular,  we  have 


z2  =  u 


R2(zx) 


A 


which  for  u  =  —  1  yields 


^2min 


Z 


4 

2 


R2(zx)' 


(30) 


(31) 
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On  the  other  hand,  the  slope  of  the  22crit(zi)  characteristic  in  (17)  is 

,  ,  ^  _  sgnR(zi)R'(z^ 

"2crit(2l)“ 

Using  (31)  and  (32)  we  may  enforce  the  inequality  z2min 

P(z2)  =  4  +  R2(zi)p2(z\)zl  - 
Solving  for  z2,  the  roots  of  P{z2)  are 


A 

ri,  2  = 


-  p(z  1). 

(32) 

<  4crit  by 

R2(zi)  <  0. 

(33) 

+  4R2(zi) 

5 

(34) 

and  for  (33)  to  hold,  given  z2  >  0,  we  must  have 

-R2{zi)p2(z1)  +  y/ R4(zi)p4(z1 )  +  4R2(z1)\ 


1/2 


22  <  \/r2  = 


(35) 


In  the  limiting  case,  when  z2mia  =  z'2cr-lV  we  have  ^2safe  =  \^2-  An  explicit  relationship  between 
22safe  and  ^2crit  is  given  by  the  equation 


"^2safe  "2crit 


R!{zif 


"2safe^2crit  0, 


(36) 


which  implies  that  z2sa{e(zi )  <  z2cT\t(zi)  for  all  z\  €  [z\,z2].  The  set  M.  is  therefore  the  area 
underneath  the  curve  Z2Base(zi)-  ^  follows  that  M.  C  S.  The  £2crit(2i)  and  z2sa{e{z{)  curves  for 
paths  of  increasing  curvature  are  shown  in  Fig.  3. 

To  finish  the  proof,  notice  that  for  any  trajectory  starting  in  S\ M.  (the  area  between  the  char¬ 
acteristics  Z2safe(2i)  and  Z2crit(-£i)  in  Fig.  3)  using  u  =  —1,  we  have  that  z'2^m  >  z2cr[t.  Integrating 
now  backwards  in  time  from  zf  to  z\  (dzi  <  0)  using  u  =  —  1  one  obtains 


d-Z2m[n  < 


rzi 


d~'2rri!  , 


(37) 


or  Z2min(2l)  -  22min(2f  )  <  Z2crit  (21)  ~  Z2cnt{zf).  Since  Z2min{zf)  <  Z2crit{zf)  for  points  ill  S\M  it 
follows  that  22min(2l)  <  Z2cxit{z\).  ®ee  a^SO  Fig-  4. 

We  conclude  that  for  an  increasing  curvature  path  P^B, 

ZB(zi)  <  22crit(zi),  Zl  €  [zi,zf),  (38) 

given  that  z2  <  z2crit(z^ ).  ■ 


Lemma  2  implies,  in  particular,  that 

22(21)  =  min{Z^(zi),ZB(zi)}  <  22crit(2i),  21  €  [z±,zf).  (39) 

Hence  the  constraint  (12)  remains  inactive  throughout  the  optimal  trajectory.  We  have  therefore 
shown  the  following  result  for  the  optimal  trajectory  on  paths  of  monotonically  increasing  curvature. 


Proposition  3  In  the  case  of  a  path  of  monotonically  decreasing  curvature  VfB,  there  can  be  at 
most  one  switching  in  the  control,  from  u  =  +1  to  u  =  —  1.  In  this  case  the  optimal  solution  is 
given  by 

22(21)  =  min  {Z^(z1),Z~(z1)}  .  (40) 


14 


z 


1 


Figure  3:  In  the  area  between  Z2crit  and  ^2safe  we  can  integrate  backward  in  time  with  u  =  —  1 
without  intersecting  *2crit- 


3.2.6  Numerical  Example 

Consider  a  path  of  increasing  magnitude  of  the  radius  (decreasing  curvature)  from  point  A  to  point 
B  as  in  Fig.  5(a),  described  by  the  following  equation 

R(zi)  =  -0.5*1  -  10.  (41) 

Assume  that  the  vehicle  starts  from  point  A  with  zero  velocity  and  reaches  point  B  with  zero 
velocity  as  well.  The  switching  point  SP  is  determined  by  the  intersection  of  the  characteristic 
Z\(z i)  in  Fig.  5(b),  created  by  forward  integration  of  the  equations  of  motion  (10)  with  initial 
conditions  (z±,  z%)  using  u  =  +1,  with  the  characteristic  (*i),  created  by  backward  integration 
of  the  equations  of  motion  with  initial  conditions  (zf ,  )  using  u  =  —1.  Equivalently,  the  optimal 
solution  z%(zi)  is  given  by 

40i)  =  min  {za(zi)’Zb(zi)}  •  (42) 

Figure  6  confirms  that  the  optimality  conditions  hold,  i.e.  the  switching  function  A2  changes  sign 
at  the  switching  point  of  the  control  input. 

3.2.7  Solution  for  Special  Cases  of  Path  Curvature:  Active  Constraint 

In  this  section  we  consider  the  case  when  the  constraint  (12)  is  active,  i.e.,  Cq{z\,Z2)  =  0-  First, 
observe  that  when  the  speed  of  the  vehicle  takes  the  critical  value  (17),  equation  (12)  implies  that 
(zi,  *2)  dS.  In  this  case  fn  =  Fmav  and  *2  =  0  from  (10).  The  control  input  u  cannot  affect  the 
value  of  the  velocity  and  loss  of  controllability  ensues.  This  may  also  be  interpreted  as  a  loss  of 
the  ability  to  generate  tangential  force  ft,  since  the  whole  force  capacity  Fmax  is  used  to  produce 
the  required  centripetal  force  fn.  From  Lemma  1  it  follows  that  this  case  is  possible  only  for  paths 
of  constant  curvature.  In  the  remaining  of  this  section  we  will  therefore  consider  only  paths  with 
R'{zi)  =  0. 
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(a) 


(b) 


Figure  4:  Optimal  solution  for  a  path  of  decreasing  curvature  near  the  state  constraint. 


In  order  to  avoid  the  difficulty  arising  from  the  loss  of  controllability  for  the  case  of  a  path  of 
constant  curvature,  we  introduce  the  constraint 


Ce(zi,z2)  =  z$  +  e-  |.R(zi)|  =  0, 


(43) 


where  e  >  0  is  a  small  positive  scalar.  We  investigate  optimal  paths  that  satisfy  this  constraint  and 
the  we  take  e  — >  0  to  recover  the  case  of  Co(zi,  z 2)  =  0  at  the  limit. 

An  easy  calculation  shows  that  the  control  law  that  keeps  the  vehicle  on  the  constraint  (43)  is 
given  by 


2v/2|i?(2l)|e-e2’ 


(44) 


which,  upon  R'  {zf)  =  0  yields  usc  =  0  for  any  e  >  0.  Hence  lim£_>.o  usc  =  0. 

Consider  now  a  path  of  constant  curvature  VAF.  Figure  7  shows  the  characteristic  ZA{zf) 
constructed  by  forward  integration  of  (10)  from  (zf,zf)  with  u  =  +1,  the  characteristic  Z\F{zf) 
given  by  Z2{z\)  =  \J\R{zf)\  —  e  <  ^2crit(^i)j  for  z  1  S  [zf,zf],  with  e  >  0,  and  the  characteristic 
Zf{zf)  constructed  by  backward  integration  of  (10)  from  {zf,zf)  with  u  =  —1.  Notice  that  the 
characteristic  Z°AF{zf)  is  constructed  with  u  =  0,  which  coincides  with  usc  in  (44)  for  R'(zi)  =  0. 
On  the  characteristic  ZAF(z\)  the  constraint  (43)  remains  active,  i.e. ,  Ce(zi,  ZAF(zi))  =  0. 


Proposition  4  In  the  case  of  a  path  of  constant  curvature  V\F,  assuming  e  >  0,  the  optimal 
solution  is  given  by 


z*2(zi)  =mm{Z^(zi),Z%(zi),ZF(zi)}  .  (45) 

The  trajectory  from  zf  to  zf  in  Fig.  7  maximizes  point- wise  the  velocity  since  it  is  constructed 
using  maximum  acceleration  u  =  +1  from  a  fixed  initial  velocity  22 (to)  =  z^-  The  velocity  of 
the  trajectory  from  zf  to  zf  is  equal  to  the  maximum  allowable  value,  -\/\R(z\)\  —  e.  Finally,  the 
trajectory  from  zf  to  zf  on  Zf  is  also  of  maximum  point- wise  velocity,  since  Zf  is  constructed  using 
maximum  acceleration  u  =  —1  backward  in  time  starting  from  a  fixed  initial  velocity  Z2(t/)  =  zf . 
Thus,  the  overall  trajectory  of  (45)  maximizes  the  velocity  point-wise,  which  proves  the  optimality 
of  the  proposed  solution. 
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(a)  (b) 

Figure  5:  Optimal  velocity  profile  through  a  path  of  increasing  radius. 

Let  now  e  — >  0  and  assume  that  at  some  point  rc  E  (to,t/)  we  have  22 (rc)  =  Z2Cmt(zi(r(f)) ,  as 
in  (17).  This  point  corresponds  to  (2p,  22Crit)  in  Fig.  7.  Since  the  square  root  in  equation  (10) 
becomes  zero,  the  tangential  acceleration  becomes  zero  and  hence  22  (tc)  =  0.  Since  we  are  on  a 
path  of  constant  curvature,  Z2 (r)  =  Z2crit(-Zi  (t))  for  all  r  >  rc  and  thus  once  controllability  is  lost, 
it  cannot  be  regained.  This  means  that  when  the  vehicle  operates  at  (z\\  22crit) ;  it  cannot  switch 
to  (21)  and  the  vehicle  continues  to  travel  with  Z2crit-  We  conclude  that  in  case  of  a  path  of 
constant  curvature  V\F,  unless  the  final  velocity  z%  =  Z2cnt{z( )  we  cannot  allow  e  =  0.  We  have 
therefore  shown  the  following  corollary. 

Corollary  2  Consider  the  minimum-time  problem  (10),  and  assume  a  path  of  constant  curvature, 
R(zi)  =  c.  If  22 (r/)  /  y/\ ~c\  an  optimal  solution  does  not  exist. 

In  the  sequel  we  investigate  paths  composed  of  concatenations  of  paths  investigated  thus  far. 
Such  concatenations  will  allow  us  to  construct  the  optimal  trajectories,  along  with  the  corresponding 
optimal  controls,  by  piecing  together  the  solutions  provided  by  Propositions  1-4. 

3.2.8  Path  with  min  R{z\ ) 

Consider  now  a  path  of  increasing  curvature  V(^c  followed  by  a  path  of  decreasing  curvature  VFB 
as  in  Fig.  8.  We  adopt  the  following  notation  for  the  path  from  point  A  to  point  B 

VACB  =  VAC°VCB  =  {(*i,-R(*l))  :  R'(zi)  sgnR(zi)  <0,  Zi  E  [zf,z^), 

R'(zi)  sgnR(zi)  >0,  zi  E  [z^zf])}  ,  (46) 

where  “o”  denotes  the  concatenation  operator.  The  function  R(z\)  has  a  minimum  at  z(f  (see 
Fig.  8). 

Let  ^2(^1)  denote  the  minimum  time  solution  from  A  to  B  and  z(f*  denote  the  velocity  at  point 
C  of  the  22(21)  trajectory.  According  to  Bellman’s  Principle  of  Optimality  if  the  solution  A  — >  B  is 
optimal  then  the  first  part  of  this  solution,  A  — >  C,  solves  the  minimum  time  problem  from  A  to  C 
with  the  final  condition  22  (r/)  =  .  Similarly,  the  second  part,  C  B,  solves  the  minimum-time 

problem  from  C  to  B  with  initial  condition  22(10)  =  z(f*  . 


17 


Figure  6:  Switching  function  and  control  input  time  history  for  the  path  shown  in  Fig.  5(a). 

The  velocity  z 2*  is  not  known  a  priori  however,  and  the  solution  A—>B  cannot  be  constructed 
from  the  solutions  A  — >  C  and  C  — ►  B.  Nonetheless,  we  do  know  the  allowable  switchings  of  the 
control  for  the  subarcs  A  — *  C  and  C  — >  B  from  the  analysis  in  Sections  3.2.4  and  3.2.5. 

On  the  part  A  — >  C  we  have  a  path  of  increasing  radius,  and  according  to  Section  3.2.4  the 
possible  optimal  velocity  profiles,  summarized  in  Fig.  9(a),  are:  u  =  + 1  (Case  1),  u  =  —  1  (Case  2) 
or  u  =  +1  that  switches  once  to  u  =  —  1  (Case  3).  Similarly,  on  the  part  C  — ►  B  we  have  a  path  of 
decreasing  radius  and  according  to  Section  3.2.5  the  possible  optimal  velocity  profiles,  summarized 
in  Fig.  9(b),  are:  u  =  +1  (Case  a),  u  =  —  1  (Case  b)  or  u  =  +1  that  switches  once  to  u  =  —  1 
(Case  c).  For  Bellman’s  Principle  of  Optimality  to  hold,  the  overall  solution  from  A  to  B  will 
consist  of  the  subarcs  A  — >  C  and  C  — >  B  that  correspond  to  Cases  1,2,3  and  Cases  a,b,c  (Fig  9), 
respectively.  Thus,  all  the  possible  optimal  velocity  profiles  for  the  overall  problem  from  A  to  B 
are  all  the  possible  combinations  of  Cases  1,2,3  and  Cases  a,b,c.  These  are  shown  in  Fig  10.  In  the 
following,  we  discuss  each  case  separately  in  order  to  compute  the  optimal  velocity  at  point  C. 

Case  la  corresponds  to  u  =  +1  in  both  subarcs,  A  — >■  C  and  C  — ►  B  and  the  optimal  solution 
z%(zi)  coincides  with  the  characteristic  Zj^(zi).  Obviously,  there  is  no  other  path  that  satisfies  the 
boundary  conditions  at  points  A  and  B.  The  velocity  at  point  C  has  to  be  less  than  or  equal  to 
2;2crit(-zi  )■  In  this  case  the  optimal  velocity  z^  is  determined  by  the  boundary  conditions  at  A  and 
B. 

Case  lb  corresponds  to  u  =  +1  from  A  — >  C  and  u  =  —  1  from  C  B.  Contrary  to  the 
previous  case,  it  is  now  possible  to  satisfy  the  boundary  conditions  at  A  and  B  using  acceptable 
control  switchings.  Consider,  for  example,  the  solution  using  the  sequence  of  characteristics  Z\  (u  = 
+1)  1  Zm  (u  =  -1),  zn  (u  =  +1),  (u  =  —1)  shown  in  Fig.  10,  Case  lb.  However,  it  is  obvious 
that  the  solution  using  one  switching  from  Z~ £  to  Z g  maximizes  velocity  point-wise  between  A  and 
B  (for  the  given  boundary  conditions),  and  thus  this  is  the  optimal  solution.  Again,  the  velocity 
at  point  C  has  to  be  less  than  or  equal  to  22 crit(^f)- 

Case  lc  corresponds  to  u  =  +1  from  A  — »  C  and  switching  of  the  control  from  u  =  +1  to 


18 


Figure  7:  Constant  radius  path;  active  constraint  case. 


u  =  —  1  along  the  subarc  C  — >  B.  This  case  is  similar  to  the  Case  lb.  The  overall  trajectory 
consists  of  one  switching  from  acceleration  to  deceleration.  However,  this  time  the  switching  does 
not  take  place  at  point  C  due  to  the  different  boundary  conditions  at  A  and  B.  Again,  the  velocity 
at  point  C  has  to  be  less  than  or  equal  to  the  critical  one. 

Case  2a  corresponds  to  u  =  —1  from  A  —*  C  and  u  =  +1  from  C  — >  B.  Assume,  as  shown  in 
Case  2a  of  Fig.  10,  that  there  are  other  solutions  that  consist  of  admissible  switchings,  and  which 
satisfy  the  same  boundary  conditions  at  points  A  and  B.  In  fact,  the  solution  that  corresponds  to 
Case  2a,  constructed  by  the  characteristic  paths  Z q  and  Zq,  is  the  one  with  the  lowest  velocity 
point-wise  between  A  and  B.  We  conclude  that  Case  2a  will  be  optimal  only  if  z =  ~2crit(zf), 
which  is  acceptable  since  a  path  of  decreasing  curvature  follows  after  point  C  and  controllability  is 
regained  immediately. 

Case  2b  corresponds  to  u  =  —  1  in  both  subarcs,  A  — >  C  and  C  — >  B.  It  is  completely 
equivalent  to  Case  la  if  we  reverse  the  boundary  conditions  at  the  points  A  and  B. 

Case  2c  corresponds  to  u  =  —1  from  A  C  and  one  switching  from  u  =  +1  to  u  =  —  lin  the 
subarc  from  C  —>  B.  As  in  Case  2a,  unless  Up*  =  Z2 crit^f)  there  are  other  solutions  that  satisfy 
the  boundary  conditions  at  points  A  and  B  consisting  of  higher  velocities  at  all  points  between  A 
and  B. 

Case  3a  corresponds  to  one  switching  from  u  =  +1  to  u  =  —  1  along  the  subarc  A  — ►  C  and  to 
u  =  +1  along  C  — >  B.  This  case  is  equivalent  to  the  Case  2c  if  we  switch  the  boundary  conditions 
of  points  A  and  B. 

Case  3b  corresponds  to  one  switching  from  u  =  +1  to  «  =  -1  along  the  subarc  A  — >  C  and 
to  u  =  —1  from  C  — >  B.  It  is  equivalent  to  Cases  lb  and  lc;  however,  in  this  case  the  switching  of 
control  occurs  before  point  C. 

Case  3c  corresponds  to  one  switching  from  u  =  +1  to  u  =  —1  along  the  subarc  A  — >  C  and 
switching  from  u  =  +1  to  u  =  —  1  along  the  subarc  C  — >  B.  Unless  z%*  =  Z2crit(-2p)  there  are 
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Figure  8:  Path  with  minimum  radius  at  point  C,  in  cartesian  coordinates  (left);  path  radius  as  a 
function  of  path  length  (right). 


other  solutions  that  satisfy  the  boundary  conditions  at  points  A  and  B,  which  all  consist  of  higher 
velocity  at  all  points. 

From  the  previous  analysis  we  conclude  that  there  are  only  two  possible  scenarios  for  the  value 
of  zf*.  In  Cases  la,  2a,  3a,  2b  and  3b  we  have  zf*  <  Z2Crit(zf)  and  z\c  determined  by  satisfying 
the  boundary  conditions  of  A  and  B  using  allowable  control  switches.  In  Cases  2a,  2c,  3a  and  3c 
we  have  z f  *  =  Z2Crit(zf )  an<4  a  control  switch  from  u  =  —  1  to  u  =  +1  at  C. 

Next,  we  propose  a  methodology  to  construct  the  overall  optimal  solution  for  a  path  with 
curvature  switching  from  increasing  to  decreasing  at  a  point  C.  Starting  from  (zf ,  zf )  we  construct 
the  characteristic  Zf  (zf  integrating  the  equations  of  motion  (10)  forward  in  time  using  u  =  +1. 
Starting  from  (zf  ,  zf)  we  construct  the  characteristic  Zg(zi)  integrating  the  equations  of  motion 
(10)  backward  in  time  using  u  =  —1.  Starting  from  (zf,Z2Crit(z f))  we  construct  the  characteristic 
Zc(Zl)  integrating  the  equations  of  motion  (10)  backward  in  time  using  u  =  —1.  Finally,  starting 
from  (zf ,  Z2crit(zf ))  we  construct  the  characteristic  Z@(zi)  integrating  the  equations  of  motion 
(10)  forward  in  time  using  u  =  +1.  The  optimal  velocity  profile  is  then  given  by 

4(*i)  =  min{ZA(zi)’Zc(zi),zc(zi),ZB(zi)}  ■  (47) 

It  is  easy  to  show  that  (47)  reproduces  all  the  cases  of  Fig.  10. 

3.2.9  Path  with  maxi?(zi) 

Consider  now  a  path  of  decreasing  curvature  V\c  followed  by  a  path  of  increasing  curvature  V^B . 
We  adopt  the  following  notation  for  the  path  from  point  A  to  point  B 

'Pacb  =  T-’ac  °  ^ cb ■  (48) 

Clearly,  in  this  case  the  function  R(zi)  has  a  maximum  at  zf.  All  possible  scenarios  that  may 
appear  along  the  subarcs  A  — >  C  and  C  — >  B  according  to  the  solutions  presented  in  Sections  3.2.4 
and  3.2.5  may  be  summarized  in  accordance  to  Fig.  9. 

In  Section  3.2.8  we  concluded  that  Cases  2a,  2c,  3a  and  3c  may  appear  as  the  optimal  solutions 
only  if  the  velocity  at  C  is  zf*  =  Z2Crit(zf).  Since  C  is  a  point  of  maximum  radius,  the  critical 
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(a)  A  ->  C  (b)  C  ->  B 


Figure  9:  Possible  optimal  velocity  profiles  before  (left)  and  after  (right)  point  C. 


velocity  at  point  C  is  larger  compared  to  any  other  point  from  A  to  B.  That  is, 

32crit(2l)  <  Z2crit(zf)  for  Zi  G  [z±  ,  zf )  U  (zf,zf].  (49) 

On  the  other  hand,  in  Cases  2a,  2c,  3a  and  3c  the  velocity  at  point  C  is  a  local  minimum.  That  is, 
there  exists  8  >  0  such  that  z <  22(21)  for  z\  G  (zf  —  8,  z\  +  S).  For  =  Z2ait{zi)  equation 
(49)  implies  that  22(21)  >  22Crit(-zi ),  for  all  z\  G  (zf  —  L  zp  +  5),  and  the  vehicle  cannot  follow  the 
prescribed  path.  We  conclude  that  Cases  2a,  2c,  3a  and  3c  cannot  appear  as  optimal  solutions  in 
the  case  of  a  path  with  a  point  C  of  maximum  radius. 

The  only  possible  scenarios  are  Cases  la,  lb,  lc,  2b  and  3b,  where  the  optimal  velocity  at  C  is 
determined  by  the  initial  and  final  boundary  conditions.  The  optimal  solution  is  finally  given  by 

22(21)  =  Tain{Z\{z1),Z^{zi)} .  (50) 


3.2.10  General  Solution 


Assume  that  the  given  path  from  point  A  to  point  B  is  composed  of  a  finite  number  of  segments  of 
constant  curvature,  of  segments  of  monotonically  increasing  curvature  and  segments  of  monotoni- 
cally  decreasing  curvature.  Let  the  total  number  of  segments  be  n  +  1.  The  path  from  point  A  to 
point  B  can  then  be  expressed  as 
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(51) 


where  i}-  G  {+,  — ,  0},  k  =  1,2, ...,  n+1.  Let  X ^  denote  the  set  of  indices  corresponding  to  points  of 
minimum  radius  of  the  path  Tab ,  that  is,  XT  =  {  j  :  ij  =  — ,  ij+ 1  =  +},  T°  denote  the  set  of  indexes 
corresponding  to  the  first  point  of  a  segment  of  constant  curvature,  i.e.  T°  =  {j  :  ij  =  0,  ij- 1  /  0}. 

The  following  algorithm  provides  an  e-suboptimal  velocity  profile  for  minimum  time  travel  along 
the  path  Vab- 
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(m/sec)  z  (m/sec)  z  (m/sec) 


Figure  10:  All  possible  optimal  velocity  profiles  from  A  to  B. 


ALGORITHM  FOR  OPTIMAL  VELOCITY  PROFILE 

•  From  ( z i,  z £)  integrate  the  equations  of  motion  (10)  forward  in  time  with  u  =  +1 
to  construct  the  characteristic  Z\{z\). 

•  From  (zi  ,  integrate  the  equations  of  motion  (10)  backward  in  time  with  u  =  —  1 
to  construct  the  characteristic  Zb(z  i). 

•  For  each  point  of  minimum  radius  Pk,  k  G  2T,  construct  the  following  charac¬ 
teristics:  the  characteristic  Zp  (z\)  by  integrating  (10)  backward  in  time  from 

(zfk,  Z2cr\t{z^k))  using  u  =  —1,  and  the  characteristic  Zp  (zi)  by  integrating  (10) 
forward  in  time  from  (z{fe,  22crit(~ffc))  using  u  =  +1. 

•  For  each  segment  Ppepl+1  of  constant  radius  R(zi)  =  where  l  G  1°,  construct  the 
following  characteristics:  the  characteristic  Zp(Pi+i(zi)  of  constant  velocity  equal 
to  \/\R(\  —  e,  for  some  e  >  0,  the  characteristic  (zi)  by  integrating  backward 
in  time  from  (zf'6,  \f\Rn\  —  e)  using  u  =  —1,  and  the  characteristic  Zp  (z-\)  by 
integrating  forward  in  time  from  (zf/+1,  \/\Ri\  —  e)  using  u  =  +1. 

•  The  solution  to  the  minimum  time  problem  is  given  by 

A  (2i)  =  min  {za(2i)’Zb(zi)>  Zpk(z  i),  Z+(*i),  Z%tPt+1(zi),  Zpt(zi),  Z++I(zi)}  , 

(52) 

where  k  G  and  £  G  X°. 
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The  previous  algorithm  will  give  the  optimal  velocity  profile  in  case  1°  =  0.  Otherwise,  and 
in  light  of  Corollary  2,  the  solution  is  suboptimal  in  the  sense  that  it  can  always  be  improved  by 
taking  e  — >  0  but  not  zero.  Proof  of  optimality  can  be  easily  provided  by  showing  that  this  solution 
maximizes  the  velocity  pointwise.  We  demonstrate  this  fact  in  the  following  example. 

3.2.11  Example  (General  Solution) 

Consider  the  path  Vab  shown  in  Fig.  11(a).  We  can  identify  points  of  minimum  radius  at  Pi,  P4 
and  P9  (1^  =  {1,4,9}),  and  intervals  of  constant  radius  Vp5p6  and  Vp7p&  (1°  =  {5,7}).  Figure 
11(b)  shows  the  construction  of  the  necessary  characteristics  using  the  rules  of  the  previous  section. 
The  minimum  time  solution  is  given  by  (52)  and  it  is  shown  in  Fig.  12(a). 


z/m)  z1  (m) 

(a)  (b) 


Figure  11:  (a)  A  general  case  radius  profile  path;  (b)  the  free  boundary  conditions  problem  solutions 
for  constant  radius  and  rninP  subarcs. 

Consider  now  the  intervals  (i)  -  (viii)  along  the  optimal  solution  as  in  Fig.  12(b).  In  the  interval 
(i)  we  use  maximum  acceleration  u  =  +1  from  the  starting  point  A  and  thus  the  velocity  is 
maximized  point-wise  in  the  interval  (i).  In  (ii)  the  vehicle  decelerates  with  u  =  —  1  towards  the 
critical  velocity  at  P4.  A  trajectory  passing  from  a  point  of  higher  velocity  in  (ii)  would  violate 
the  constraint  (43)  at  P4.  After  P4  we  have  maximum  acceleration  and  thus  point- wise  maximum 
velocity  in  (iii).  The  velocity  in  (iv)  is  equal  to  the  maximum  allowable  from  (43).  In  (v)  we  have 
maximum  acceleration  and  thus  maximum  velocity  as  in  (iii) .  Point-wise  maximality  of  the  velocity 
in  (vi)  and  (vii)  is  shown  in  accordance  to  (ii)  and  (iii)  respectively.  Considering  the  problem  from 
B  to  A,  the  trajectory  in  (viii)  corresponds  to  maximum  acceleration  from  the  fixed  condition  at 
B  and  thus  it  maximizes  the  velocity  point-wise. 

We  conclude  that  the  velocity  is  maximized  point-wise  throughout  the  trajectory  from  A  to  B, 
and  thus  the  trajectory  computed  using  (52)  is  the  solution  to  the  minimum  time  problem. 

3.2.12  Application  to  an  FI  circuit 

In  this  section  we  validate  the  proposed  methodology  by  applying  it  to  an  actual  road  track. 
Specifically,  we  use  the  previous  methodology  in  order  to  generate  the  optimal  velocity  profile  over 
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(a)  (b) 

Figure  12:  Optimal  velocity  profile  for  the  general  case  path  of  Fig.  11. 


a  FI  circuit,  given  the  acceleration  limits  of  a  typical  FI  race  car.  The  results  are  compared  to  the 
velocity  profiles  and  lap  times  achieved  by  expert  FI  race  drivers. 

Figure  13(a),  taken  from  [17],  shows  the  cartesian  coordinates  of  the  Silverstone  FI  circuit. 
The  data  of  Fig.  13(a)  were  used  to  generate  the  curvature  profile  of  this  trajectory,  which  is 
shown  in  Fig.  13(b).  By  matching  the  performance  characteristics  of  the  vehicles  in  [2],  [17]  we  can 


(a)  (b) 


Figure  13:  A  trajectory  followed  by  an  FI  race  car  in  the  Silverstone  FI  circuit  [17]  and  its  curvature 
profile. 


approximate  the  acceleration  limits  of  a  typical  FI  race  car  as  follows 


f  +16  —  0.0021u2  m/sec2  for  u  =  +1, 
\  —18  —  0.0021u2  m/sec2  for  u  =  — 1, 


and 


/™ax/m  =  30  rn/sec2. 
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The  optimal  velocity  profile  along  the  trajectory  of  Fig.  13  is  calculated  using  the  methodology 
of  Section  3.2.10.  The  results  are  shown  in  Fig.  14(b).  Figure  14(a),  taken  from  [17],  shows  the 
velocity  measurements  for  three  laps  of  an  FI  car  along  the  Silverstone  circuit.  The  optimally 
calculated  lap  time  using  the  proposed  approach  is  82.7  sec.  The  measured  lap  times  corresponding 
to  the  data  of  Fig.  14(a)  are  86.063  sec,  90.891  sec  and  85.805  sec  respectively  for  each  lap.  Note  that 
the  record  time  for  the  Silverstone  circuit  belongs  to  K.  Raikkonen  (78.233  sec,  McLaren  Mercedes, 
2004). 


Figure  14:  Velocity  profiles  through  the  Silverstone  circuit:  (a)  achieved  by  human  driver,  (b) 
computed  optimal. 


3.2.13  Receding  Horizon  Implementation 

When  the  environment  is  changing  the  optimal  profile  needs  to  be  generated  on-line.  One  way  to 
achieve  this  is  for  the  trajectory  optimization  to  be  implemented  in  a  receding  horizon  scheme  rather 
than  executed  in  one  shot,  from  the  start  point  to  the  end  point.  In  [12]  numerical  optimization 
along  with  a  receding  horizon  scheme  was  used  for  trajectory  planning  of  an  autonomous  vehicle 
maneuvering  through  obstacles.  By  including  the  distance  between  the  end  of  the  horizon  and  the 
final  destination  point  in  the  total  cost,  it  was  shown  that  the  vehicle  reaches  the  final  point.  In  [13] 
an  extension  of  the  previous  optimization  scheme  was  proposed  in  order  to  avoid  the  entrapment 
of  the  vehicle  in  concave  obstacles.  The  main  idea  is  that  the  cost  function  is  estimated  off-line 
for  the  whole  area  where  the  vehicle  may  move.  Areas  that  may  lead  to  entrapment  are  penalized 
and  the  estimated  cost  is  taken  into  consideration  in  the  total  cost.  Finally,  in  [14]  the  receding 
horizon  strategy  of  [12]  was  combined  with  a  “safety  algorithm”.  The  “safety  algorithm”  computes 
an  “escape  plan”  from  the  end  of  the  horizon  to  a  “safe”  state  (such  as  the  vehicle  coming  to  a 
stop),  for  each  optimization  step.  If  such  an  “escape  plan”  is  not  feasible,  then  the  last  optimization 
step  is  not  executed  and  the  “escape  plan”  of  the  previous  step  is  executed  instead. 

In  this  work  we  have  assumed  that  the  geometry  of  the  trajectory  is  computed  separately 
and  it  is  provided  to  the  velocity  optimizer  beforehand.  Therefore,  during  a  receding  horizon 
implementation  we  assume  that  it  is  the  job  of  the  path  planner  to  provide  a  feasible  path  that 
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ensures  obstacle  avoidance  and  guarantees  that  the  vehicle  will  reach  its  final  destination.  This 
can  be  achieved  by  following  the  same  strategy  as  in  [12], [13]  and  [14].  However,  guarantees  that 
the  velocity  will  not  exceed  the  critical  value  at  any  point,  and  that  there  exists  an  “escape  plan” 
at  the  end  of  each  optimization  step,  will  have  to  be  provided  before  the  velocity  optimizer  is 
implemented  in  a  receding  horizon  scheme.  Below  we  propose  a  dynamic  scheme  to  adaptively 
choose  the  planning  and  execution  horizons  to  provide  such  guaranties. 

3.2.14  Receding  Horizon  Scheme 

Figure  15  shows  a  schematic  that  demonstrates  how  the  receding  horizon  scheme  works.  The 
Planning  Horizon  (PH*)  is  the  distance  from  the  current  position  up  to  the  point  which  the  ith 
optimization  step  is  performed.  The  Execution  Horizon  (EH;)  is  a  fraction  of  PH;  and  it  is  the 
distance  up  to  the  point  which  the  planned  optimization  will  actually  be  executed.  When  the  vehicle 
reaches  the  Replanning  Horizon  RH;,  which  is  a  fraction  of  EH;,  the  optimization  is  performed  again 
up  to  the  new  planning  horizon  PH;+i. 
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Figure  15:  Optimization  with  receding  horizon. 

Let  the  vehicle  be  at  the  starting  point  (s  =  so  in  Fig.  15).  The  optimization  methodology 
is  applied  up  to  PHi.  After  the  optimization  is  completed,  the  vehicle  may  start  executing  the 
optimal  trajectory  up  to  EHi.  The  shadowed  area  (i)  in  Fig.  15  shows  the  portion  of  the  first 
optimization  that  is  actually  executed.  When  the  vehicle  reaches  RHi  the  optimization  is  applied 
again  from  the  current  position  RHi  to  the  new  planning  horizon  PH2.  The  portion  of  the  second 
optimization  that  will  be  actually  executed  is  from  EHi  to  EH2  and  it  is  shown  as  the  shadowed  area 
(ii)  in  Fig.  15.  The  process  will  end  when  the  final  destination  is  within  the  execution  horizon.  The 
distance  between  RH;  and  EH;  is  chosen  such  that  enough  time  is  allowed  for  the  computation  of  the 
optimal  trajectory  from  RH;  to  PH;+i  before  EH;  is  reached.  If  the  computation  is  instantaneous 
RH;  and  EH;  can  coincide.  As  already  mentioned,  the  semi-analytical  nature  of  the  proposed 
algorithm  results  in  minimal  computational  cost  and  thus  from  now  on  we  will  assume  that  the 
replanning  and  execution  horizons  coincide. 
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3.2.15  Robustness  guarantees 

In  this  section  we  propose  an  implementation  scheme  for  the  receding  horizon  optimization  of  the 
velocity  profile  along  a  given  path.  In  particular,  we  propose  a  dynamic  scheme  to  determine 
planning  and  execution  horizons  in  the  Z\  domain  and  guarantee  the  existence  of  an  “escape  plan” 
in  the  end  of  each  executed  subarc. 

We  propose  the  following  formula  to  determine  the  planning  horizon  of  the  optimization 
step 


PR  =  max{Tuj,  PHmin},  (53) 

where  Vi  is  the  vehicle  velocity  at  the  position  of  the  execution  horizon  of  the  previous  optimization 

step  EHj_i,  Vi  =  Z‘2 (EHj _ } ) ,  T  is  a  constant  “reaction”  time,  and  PHmin  is  the  minimum  planning 

horizon  (typically  for  Vi  =  0).  This  is  not  unlike  the  way  a  human  driver  chooses  a  planning  horizon, 
that  is,  the  larger  the  velocity,  the  longer  the  “look  ahead”  distance  needs  to  be.  At  the  initial 
point  A  on  the  path,  for  the  first  optimization  step  i  =  1  we  have 

EH0  =  zf.  (54) 

The  optimal  solution  from  the  current  position  EH,_i  to  PHj  is  calculated  using  (52)  and  is  denoted 
by  ^Oi)- 

Next,  we  construct  the  characteristic  from  =  PHj,  =  0)  integrating  backwards  in  time 
using  u  =  —  1.  This  characteristic  is  denoted  by  lz^c{z\)  and  is  referred  to  as  escape  trajectory  of 
the  ith  optimization  step.  We  choose  the  execution  horizon  for  the  zth  optimization  step  as  follows: 

EHj  =  {Zl  :  lz*2{zi)  =  ^SC(H),  G  [EHj_1;PHj]}  .  (55) 


In  the  case  when 

{{zl:z*2{zl))1zl  €  [EHj_1,PHj]}f|{(2i>rc(^i))^i  €  [EHj_i, PHj]}  =  0  (56) 

we  need  to  increase  T  in  (53)  to  determine  a  longer  planning  horizon  until  we  can  find  the  inter¬ 
section  point  of  the  optimal  solution  and  the  escape  trajectory  (55). 

At  the  end  of  each  executed  subarc  EHj  we  optimize  up  to  the  new  planning  horizon  PHj+i. 
The  vehicle  can  decelerate  enough  to  negotiate  any  corner  outside  PHj  since  we  have  guaranteed 
that  the  vehicle  starting  from  EHj  can  come  to  a  complete  stop  at  PHj.  In  case  an  obstacle  exists 
after  PHj  the  vehicle  can  follow  the  escape  trajectory  to  avoid  collision.  The  Receding  Horizon 
optimization  scheme  terminates  when  the  end  point  B  is  within  the  execution  horizon,  namely 
zf  <  EHj  for  some  i. 

The  proposed  implementation  is  summarized  in  the  block  diagram  of  Fig.  15.  The  first  opti¬ 
mization  step  using  the  above  Receding  Horizon  scheme  on  the  general  case  path  of  Section  3.2.11 
is  shown  in  Fig.  17.  Let  the  planning  horizon  PHi  from  (53)  be  at  30m  as  in  Fig.  17.  The  execution 
horizon  EHi  is  determined  by  the  intersection  of  lz%  and  b:|sc.  Observe  that  the  solution  generated 
by  this  implementation  coincides  with  the  infinite  horizon  solution  of  Section  3.2.11.  If  we  randomly 
choose  PHj,  and  EH^  as  in  Fig.  17  we  run  the  risk  of  reaching  unacceptably  high  velocities.  For 
example,  the  critical  velocity  at  Pi  does  not  allow  the  vehicle  to  decelerate  enough  and  negotiate 
the  sharp  turn  at  P2.  In  this  case  notice  that  there  is  no  intersection  point  between  the  optimal 
solution  and  the  escape  trajectory  (characteristic  xz^c )  and  thus  we  need  to  increase  T  and  choose 
a  longer  initial  planning  horizon. 
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Figure  16:  Receding  Horizon  implementation  block  diagram. 

3.2.16  Numerical  Example  (Receding  Horizon  Implementation) 

In  this  section  we  apply  the  proposed  receding  horizon  algorithm  to  the  FI  car  trajectory  of  Section 
3.2.12.  We  have  chosen  T  =  5  sec,  which  for  this  example  is  enough  for  the  “emergency  stop” 
characteristic  to  intersect  the  optimal  solution  within  the  planning  horizon  for  each  iteration.  The 
minimum  planning  horizon  was  chosen  as  PHmjn  =  200  nr. 

In  Fig.  18  the  results  of  the  first  five  steps  of  the  receding  horizon  scheme  are  shown,  along  with 
the  planning  and  execution  horizons  of  each  step.  The  solution  (solid  line)  is  compared  with  the 
infinite-horizon  solution  of  the  optimal  velocity  generator  of  Section  3.2.12  (dotted  line).  The  two 
solutions  coincide,  thus  confirming  the  validity  of  the  proposed  receding  horizon  scheme. 
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Figure  17:  Dynamic  scheme  for  determination  of  EH*. 
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Speed  (km/h) 


Figure  18:  Optimization  with  receding  horizon  for  the  Silverstone  circuit. 
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3.3  Extension  to  the  Half-Car  Model 


Our  next  goal  is  to  extend  the  algorithm  presented  above  for  optimal  velocity  profiles  to  a  half-car 
model,  which  includes  the  yaw  dynamics  of  the  vehicle.  The  key  ingredients  for  such  an  extension 
are  the  characterization  of  the  acceleration  envelope  of  the  half-car  model  and  the  calculation  of 
maximum  acceleration  and  deceleration  on  a  prescribed  path. 

3.3.1  The  Half-Car  Model 

The  equations  of  motion  of  a  half-car  model  along  a  prescribed  path  R(s)  as  in  Fig.  19,  are  given 
below 


mx 

=  Ufx  +  fax)  cos  ip  -  ( fFy  +  fRy )  sin  ip, 

(57) 

my 

=  Ufx  +  fRx)  sin  Ip  +  ( f F y  +  fRy )  COS  Ip, 

(58) 

Iz'ip 

=  f Fy^-F  —  f Ry^-R- 

(59) 

In  the  above  equations  m  is  the  vehicle’s  mass,  Iz  is  the  polar  moment  of  inertia  of  the  vehicle, 
and  x  and  y  are  the  cartesian  coordinates  of  the  center  of  mass  (C.M.)  in  the  inertial  frame  of 
reference;  ip  is  the  yaw  angle  of  the  vehicle,  and  fij  (i  =  F.  R,  j  =  x,  y)  denote  the  friction  forces  of 
the  front  and  rear  wheels,  respectively,  along  the  longitudinal  and  lateral  body  axes.  Equations  (2) 
will  also  be  used,  with  ds/dt  =  v  =  \] x1  +  y2,  and  ft,  fn  the  components  of  the  resultant  force,  due 
to  front  and  rear  wheel  friction,  along  the  tangential  and  normal  directions  of  travel  respectively. 

The  path  angle  cp  and  the  vehicle  slip  angle  /3  (see  Fig.  19)  are  given  by 

<p  =  arctan  (y/x)  ,  (3  =  (p  —  ip,  (60) 


respectively. 


Figure  19:  A  half-Car  Model  of  a  vehicle  driving  along  a  prescribed  path  R(s). 
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3.3.2  Acceleration  Envelope  of  the  Half-Car  Model 

The  equations  of  motion  (57)-(59)  are  expressed  in  terms  of  the  friction  forces  generated  by  the 
front  and  rear  tires  fix  and  fiy,  i  =  F,  R.  In  the  following  calculation  of  the  acceleration  envelope  of 
the  half-car  model  we  neglect  power  limitations  from  the  engine/transnrission.  Such  an  assumption 
is  more  realistic  when  the  vehicle  is  operating  on  surfaces  of  low  friction  coefficient,  such  as  wet 
road  or  dirt,  where  the  adhesion  limits  of  the  tires  are  considerably  reduced  and  dominate  in  the 
calculation  of  the  overall  acceleration  capacity  of  the  vehicle. 

The  tire  friction  forces  are  calculated  using  Pacejka’s  “Magic  Formula”  model  [18]  as  follows. 

fijre  =  ~—Fi,  i  =  F,R  and  j  =  x,y,  (61) 

where,  /hre  (i  =  F,R  and  j  =  x,  y )  are  the  components  of  the  front  and  rear  wheel  friction  forces 
along  the  longitudinal  and  lateral  tire  axes  respectively,  slx  is  the  longitudinal  and  Siy  is  the  lateral 
slip  of  the  i  wheel.  The  components  /bre  should  not  be  confused  with  the  components  fij  of  the 
same  forces  along  the  longitudinal  and  lateral  body  axes,  used  in  equations  (57)-(59).  In  particular, 

fRj  =  fwje,  j  =  x,y  (62) 

f  Fx  =  If™  cos  s  -  If™  sin  5  (63) 

fFy  =  If’x  sin  S  +  fpTy  cos  5,  (64) 

where  5  is  the  steering  angle  of  the  front  wheel. 

The  total  friction  force  of  the  front  and  rear  wheel,  Fj  (i  =  F,R ),  is  computed  using 

Fi  =  F{ZD  sin(Catan(Bsi)),  i  =  F,  R,  (65) 

where  F),  (i  =  F,  R)  is  the  vertical  load  at  the  front  and  rear  axle,  respectively,  and  the  total  slip 
Si  (i  =  F,  R)  is  computed  as 


-  \]six  +  siy  with  i  =  F,R.  (66) 

The  friction  force  of  each  wheel  lies  within  a  circle  of  radius  equal  to  the  maximum  friction  force 
ymax  ^  =  F,R),  attained  at  sf121*,  from  (65).  This  is  shown  in  Fig.  20. 

We  assume  in  the  sequel  that  we  can  control  the  longitudinal  slip  slx ,  of  the  front  and  rear  wheel 
independently,  as  well  as  the  steering  angle  5  of  the  front  wheel.  Independent  longitudinal  slip  of 
front  and  rear  wheels  is  a  realistic  assumption  for  modern  vehicles  equipped  with  variable  ratio 
torque  distribution  systems  such  as  Acura’s  SH-AWD  and  BMW’s  XDrive.  Similarly,  independent 
longitudinal  front  and  rear  wheel  slip  control  can  be  achieved  by  expert  rally  drivers  via  advanced 
driving  techniques,  such  as  “left  foot  braking”  and  “handbrake  cornering”  [4],  Using  the  standard 
definition  of  longitudinal  slip  [18]  we  choose  SiX  E  [— 1,+1]. 

The  expressions  for  the  lateral  slip  of  the  front  and  rear  wheels  can  be  computed  from 


A 

sRy  ~ 
A 

SFy  = 


v  sin  f3  — 
v  cos  f3  ’ 

v  sin(/I  —  5)  +  Tptp  cos  5 
v  cos(/3  —  5)  +  ip  ip  sin  5 


(67) 

(68) 


The  rear  lateral  slip  spy  is  determined  solely  by  the  states  of  the  system,  i.e. ,  srv  is  fixed  for  a 
given  operating  condition  of  the  vehicle  (v,  (3,  i/j).  Thus,  for  a  given  operating  condition  of  the 
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Figure  20:  Total  friction  force  of  the  zth  wheel  with  respect  to  the  combined  slip  as  given  by  the 
Magic  Formula. 

vehicle,  and  assuming  that  we  can  control  the  rear  longitudinal  slip,  the  rear  friction  force  /r  lies 
on  a  characteristic  curve  Tr(srv)  as  in  Fig.  21.  That  is, 

,/r  =  (fRx j  f Ry)  G  Tr(srv).  (69) 

The  front  lateral  slip  spy,  however,  depends  on  the  steering  angle  <5,  which  is  one  of  the  control 
variables.  In  Fig.  22  we  demonstrate  that  for  any  vehicle  operating  condition  we  may  generate  any 
front  wheel  lateral  slip,  sfv  E  [— s™ax,  using  a  steering  angle  5  within  a  realistic  range  of 

5  E  [— 7t/4,  +7t/4].  In  Fig.  21  it  is  also  demonstrated  that  the  whole  friction  circle  including  its 
interior  can  be  constructed  by  characteristics  of  s py  in  the  interval  [— s™ax,  +s™ax].  Thus,  given 
any  operating  condition  of  the  vehicle,  and  assuming  that  we  can  control  independently  the  front 
longitudinal  slip  and  steering  angle,  the  front  friction  force  fp  may  be  chosen  anywhere  inside  the 
front  wheel  friction  circle  Tf 

If  =  Ufx,  f Fy)  G  Tf  =  {If  ■  |/f|  <  ffax}-  (70) 

In  Fig.  21  we  demonstrate  the  case  of  a  neutrally  balanced  vehicle  (the  C.M.  in  the  middle  of  the 
wheelbase)  with  same  tires  in  front  and  rear  wheels.  In  a  neutrally  balanced  vehicle  Fpz  =  Frz 
in  (65)  and  /j?ax  =  assuming  same  tires  in  front  and  rear  wheels.  Thus,  for  such  a  vehicle 

configuration,  we  conclude 

Tr(srv)  C  Tp  and  Tf  =  Tr(srv),  srv  E  [-•S/T'S  Sr3*].  (71) 

sRy 

At  this  point  we  assign  Jr  and  fp  from  (69)  and  (70)  respectively  to  be  the  control  variables  of 
our  system. 

The  resultant  force  envelope  Tgg(srv )  at  the  C.M.  of  the  vehicle,  referred  to  as  GG-diagram 
in  the  vehicle  dynamics  literature  [19],  is  constructed  for  each  operating  condition  of  the  vehicle  v, 
(3,  iji  (or  equivalently  srv),  by  adding  all  available  front  and  rear  tire  friction  forces,  as  in  Fig.  23. 
This  operation  is  nothing  more  than  the  Minkowski  sum  [20]  of  the  the  front  friction  circle  Tf  and 
the  rear  wheel  friction  characteristic  curve  Tr(srv),  namely, 

Tgg(srv)  =  Tf®  Tr(srv)  =  {fee  =  ,/f  +  /r,  If  G  Tf,  fn  G  Tr(srv)}  ,  (72) 
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Figure  21:  Front  and  rear  tire  friction  characteristic  curves  for  fixed  lateral  slip  Siy  (i  =  F,  R )  and 
longitudinal  slip  slx  G  [— 1,+1]  ( i  =  F,R). 

where  fGG  =  (fGGx,  fcGy)  is  the  resultant  force  at  the  C.M.  and  its  components  along  the  longi¬ 
tudinal  and  lateral  vehicle  body  axes,  fGGx  and  fGGy  respectively. 

The  optimal  control  strategy  of  Section  3.2.10  dictates  that  for  minimum  time  travel  the  vehicle 
should  use  the  maximum  available  acceleration  or  deceleration.  This  corresponds  to  u  =  ±1  in  the 
case  of  the  point  mass  model  of  Section  3.2.10.  The  maximum  available  acceleration  for  the  half 
car  model  is  given  by  the  boundary  of  FGG(sRy)  denoted  by  dFGG(sRy).  Notice  that  for  any 
fGG(sRy)  G  9FGG(sRy)  there  exists  a  unique  vector  [fp(sRy),  f^Ry)],  where  fp(sRy)  G  Ff  and 
fik(sRy)  G  Fr(sRv)i  such  that  fGG(sRy )  =  f*F{sRy)  +  f*R{sRy).  In  other  words,  one  can  define  the 
following  one-to-one  mapping 

M  :  Ff  x  Fr(srv)  i->  dFGG(sRy) 

f<GG(sRy)  =  MtfpisRy),  fR(sRy))  (73) 

and  its  inverse 

{fF(sRy)’fll(sRy))  =  -M  1(fcG(sRy))-  (74) 

An  extension  of  the  optimal  control  strategy  described  in  Section  3.2.10  becomes  now  evident. 
Given  an  operating  condition  of  the  vehicle  (velocity  components  x  and  y,  orientation  ^  and  yaw 
rate  ip)  and  the  geometry  of  the  path  k(s),  we  can  calculate  the  necessary  centripetal  force  fn 
from  (2)  such  that  the  vehicle  follows  the  path.  We  can  also  determine  the  tangential  and  normal 
directions  to  the  path  with  respect  to  the  orientation  of  the  vehicle  (these  are  denoted  by  et  and  en 
respectively  in  Fig.  23).  The  calculated  fn  lies  along  the  normal  direction  en  and  may  be  produced 
by  only  two  possible  total  forces  /q~q  and  on  dFGG  (Fig.  23).  The  force  f^Q  produces  an 
accelerating  tangential  force  ft ,  which  corresponds  to  the  u  =  +1  strategy  of  Section  3.2.10,  and 
f*QG  produces  a  braking  force  that  corresponds  to  the  u  =  —  1  strategy.  Using  the  map  AW1  of  (74) 

with  either  fffp,  or  we  can  determine  the  required  friction  forces  at  the  front  and  rear  wheels 

If  =  (/fx;  f Fy)  =  /f,  fn  =  ( fnx,fRy )  =  f*R  respectively  (Fig.  24).  We  can  use  these  expressions 
to  integrate  equations  (57)-(59). 
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steering  angle  5  (deg) 

Figure  22:  Front  lateral  slip  with  respect  to  steering  angle  for  given  operating  condition  of  the 
vehicle. 

3.3.3  Direct  Implementation  of  the  Point  Mass  Strategy 

In  the  previous  section  we  calculated  the  acceleration  capacity  of  a  half-car  model  and  derived  the 
maximum  available  acceleration/deceleration  when  the  vehicle  tracks  a  prescribed  path.  Next,  we 
demonstrate  a  direct  implementation  of  the  optimal  control  strategy  of  Section  3.2.10  to  a  path  with 
a  point  of  minimum  magnitude  of  radius  using  the  equations  of  motion  (57)-(59)  and  acceleration 
capacity  (72)  of  the  half-car  model.  The  solution  along  sub-arcs  of  the  overall  path  containing  a 
single  point  of  minimum  radius  is  the  main  “building  block”  in  the  construction  of  the  optimal 
solution  along  the  total  path  using  the  methodology  of  Section  3.2.10.  The  second  “building  block” 
is  the  solution  along  paths  of  constant  radius.  An  extension  of  the  optimal  control  strategy  along 
a  constant  radius  path  to  a  half-car  model  is  straightforward  (steady-state  cornering  of  constant 
velocity  [19,  21]  using  maximum  centripetal  acceleration)  and  will  be  omitted. 

Consider  the  path  of  Fig.  25(a)  with  radius  profile  as  shown  in  Fig.  25(b).  Notice  that  at 
point  P\(xp1,yp1)  of  the  path  the  radius  profile  takes  its  minimum  value  R(s)  =  Rp1  at  s  =  sp1. 
According  to  the  methodology  of  Section  3.2.10  the  velocity  of  the  vehicle  is  equal  to  ucrit  at  sp1 
such  that  the  generated  centripetal  force  matches  the  total  acceleration  capacity  of  the  vehicle. 
The  vehicle  decelerates  with  maximum  available  deceleration  {u  =  —1)  before  the  point  P\  and 
accelerates  with  maximum  available  acceleration  (u  =  +1)  after  the  point  Pi. 

Our  first  task  is  to  determine  the  critical  velocity  ucrit(spi)  of  the  half-car  model  at  Pi  such 
that  the  total  acceleration  capacity  is  used  towards  the  generation  of  the  centripetal  force.  As 
demonstrated  in  Section  3.3.2  the  acceleration  capacity  of  the  half-car  model  depends  on  its  state 
of  operation  v,  (3  and  ip.  We  conclude  that  ucrit  (spj  is  different  for  all  possible  values  of  /3  and  ip 
at  sp1.  For  consistency  we  enforce  the  following  attitude  and  attitude  rate  restrictions 

P(spi)  =  'PisPi)  =  v(sPl)/RPl.  (75) 

The  conditions  (75)  above  imply  that  the  vehicle  satisfies  the  steady-state  cornering  requirements 
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Figure  23:  GG-diagram  for  a  given  operating  condition  of  the  vehicle. 


[19,  21],  instantaneously  at  the  point  of  minimum  radius  Pi. 

Conditions  (75)  imply  that  the  lateral  slip  of  the  rear  wheel  at  Pi,  from  (67),  is 


SRy(sp i)  =  —£r/Rp1.  (76) 

The  rear  tire  force  along  the  normal  direction  n  coincides  with  fpy  at  sp1 ,  and  is  maximized  when 
fRx{sPi)  =  0.  Recall  that  we  may  use  any  front  tire  force  within  the  friction  circle  of  radius 
/max..  jn  or(jer  maximize  the  contribution  of  the  front  tire  to  the  centripetal  force  we  choose 

I/f?/(sPi)I  =  /“aX  and  fFx(sp- i)  =  0. 

Thus,  the  velocity  of  the  half-car  model  at  point  Pi  is  given  by 


^crit  (’SPi ) 


I^Pil(/Fax  +  l/py(sPi)l) 


rri 


(77) 


The  trajectory  of  the  vehicle  in  Fig.  26  is  constructed  according  to  the  optimal  control  strategy 
of  Section  3.2.10. 

The  acceleration  phase  is  constructed  by  integration  forward  in  time  of  the  equations  of  motion 
(57)  -  (59)  starting  from  point  Pi  with  initial  conditions  (75),  (77)  using  from  Section  3.3.2. 

The  braking  phase  is  constructed  by  integration  backwards  in  time  of  the  equations  of  motion 
(57)  -  (59)  starting  from  point  Pi  with  initial  conditions  (75),  (77)  using  from  Section  3.3.2. 

The  resulting  velocity  profile  is  shown  in  Fig.  27(a)  and  the  vehicle  slip  angle  /3  is  shown  in 
Fig.  27(b).  The  longitudinal,  lateral  and  total  friction  forces,  fjx ,  fiy  and  _/),  i  =  F,R,  of  the  front 
and  rear  tires  are  shown  in  figures  28(a)  and  28(b)  respectively. 

In  both  accelerating  and  braking  phases,  we  notice  an  increase  in  the  magnitude  of  the  vehicle 
slip  angle  (3  and  in  particular,  an  oversteering  tendency  of  the  vehicle.  Oversteer  occurs  when  the 
rear  wheels  reach  the  adhesion  limit  and  cannot  generate  any  additional  cornering  force,  while  the 
tires  of  the  front  axle  either  operate  away  from  the  adhesion  limit  [19].  In  case  both  front  and 
rear  tires  have  reached  their  adhesion  limit,  oversteer  occurs  when  the  yawing  moment  due  to  the 
front  tire  friction  is  greater  than  the  one  due  to  the  rear  tire  friction.  Oversteer  appears  as  a 
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Figure  24:  Using  M  1  we  can  calculate  the  front  and  rear  axle  forces  fF  and  fR  when  the  vehicle 
operates  at  the  limit  of  its  acceleration  capacity:  Maximum  deceleration  case  f  . 

“nose-in”  spin  of  the  vehicle  (Fig.  26),  i.e.  when  the  vehicle  slip  angle  develops  in  a  direction  such 
as  /3sign(i2)  <  0. 

For  the  numerical  example  of  Section  3.3.3  we  consider  equal  distribution  of  the  weight  in  front 
and  rear  axles  (If  =  iR)  and  same  tires  in  front  and  rear  axles.  For  such  a  vehicle  configuration 
(71)  holds  and  we  conclude  that 

max(\ fRy(sRy)\)  <max(\fFy(sRy)\),  (78) 

for  any  vehicle  operating  condition  v,  (3,  ip.  The  later  explains  the  oversteering  tendency  of  the 
vehicle  in  Fig.  26. 

In  Section  3.2.10  the  optimal  trajectory  along  the  total  path  is  constructed  by  concatenation 
of  the  optimal  trajectories  along  specific  sub-arcs,  namely,  sub-arcs  containing  a  point  of  minimum 
radius  and  sub-arcs  of  constant  radius.  Continuity  of  the  states  in  the  point  mass  model  case  of 
Section  3.2.10  is  achieved  by  switching  between  trajectories  at  the  intersection  points  of  the  z%(z\ ) 
profiles  of  each  sub-arc. 

In  the  half-car  model  case  the  state  vector  is  extended  to  include  the  yaw  dynamics  of  the 
vehicle  (/?,  ip).  In  constructing  a  trajectory  over  the  total  path  by  concatenation  of  trajectories 
over  sub-arcs  of  the  total  path,  we  have  to  ensure  continuity  of  all  states  (v,  (3,  ip)  at  the  switching 
points  from  one  sub-arc  trajectory  to  the  other.  A  necessary  condition  for  the  continuity  of  the 
yaw  states  at  the  switching  points  between  trajectories  is  that  the  yaw  states  remain  bounded.  In 
the  following  we  present  a  stable  implementation  of  the  methodology  of  Section  3.2.10  with  respect 
to  the  yaw  dynamics. 
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y  (m) 


(a)  (b) 

Figure  25:  (a)  A  corner  with  a  point  of  minimum  radius,  (b)  Radius  profile  of  the  corner. 


Figure  26:  Direct  implementation  of  the  optimal  control  strategy  to  the  half-car  model:  trajectory. 
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(a)  (b) 


Figure  27:  Direct  implementation  of  the  optimal  control  strategy  to  the  half-car  model:  (a)  velocity 
profile  (b)  vehicle  slip  angle. 


(a)  (b) 


Figure  28:  Direct  implementation  of  the  optimal  control  strategy  to  the  half-car  model:  front  and 
rear  tire  forces. 
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3.3.4  Stable  Implementation  of  the  Point  Mass  Methodology 

Vehicle  yaw  stability  characteristics  vary  with  the  different  possible  configurations  of  the  vehicle: 
weight  distribution,  front-rear-all  wheel  drive,  etc  [19].  In  this  work  we  demonstrate  the  importance 
of  yaw  stability  in  relation  to  the  problem  of  generating  near-minimum-time  velocity  profiles  along 
prescribed  paths,  rather  than  address  the  problem  of  vehicle  stability  for  all  possible  configurations. 
For  demonstration  purposes  we  will  consider  the  neutrally  balanced  vehicle  of  Section  3.3.3  with 
same  tire  characteristics  and  an  inherent  oversteering  behavior  as  described  therein. 

In  this  section  we  design  a  control  scheme  in  which  the  optimal  fpjpj  strategy  is  interrupted 
momentarily  by  a  control  law  aiming  to  reduce  the  magnitude  of  the  vehicle  slip  angle.  We  define 
the  objective  of  the  stabilizing  control  law  as  follows:  (i)  guarantee  that  the  vehicle  remains  on  the 
prescribed  path,  and  (ii)  generate  yaw  moment  to  oppose  oversteer. 

3.3.5  Stabilizing  Control 

Consider  the  following  control  strategy.  Let  both  forces  on  the  front  and  rear  axles  be  parallel  to 
the  normal  direction  to  the  path,  i.e. 

(fi,et)  =  0,  i  =  F,R  (79) 

where  (., .)  denotes  the  vector  inner  product  and  et  is  the  unit  vector  along  the  tangential  to  the 
path  direction. 

In  order  for  the  vehicle  to  remain  on  the  prescribed  path  R(s)  we  require  that  the  total  force, 
which  lies  on  the  normal  direction  to  be  equal  to  the  centripetal  force 

2 

mir 

f F  T  fR  =  fn  =  w  c  (80) 

R(s) 

where  v  is  the  current  speed  of  the  vehicle.  It  is  obvious  that  the  forces  generated  by  the  front  and 
rear  tires  contribute  only  to  the  centripetal  acceleration  of  the  vehicle  and  thus  the  speed  remains 
constant. 


v  =  0.  (81) 

The  operation  of  the  control  strategy  (79),  (80)  is  demonstrated  in  Fig.  29.  For  a  given  operating 
condition  of  the  vehicle  (v,  (3,  ip)  the  rear  tire  force  fRy(sRy )  lies  on  the  characteristic  curve  FR(sRy). 
Given  the  condition  (79)  the  rear  tire  friction  force  is  determined  uniquely  as  in  Fig.  29.  In  the 
same  figure  we  notice  that  the  front  tire  force  is  also  uniquely  determined  in  order  for  the  condition 
(80)  to  be  satisfied. 

Next,  we  derive  the  switching  function  that  determines  the  instances  when  the  (79),  (80)  control 
must  be  activated.  Recall  equation  (60)  which  associates  the  vehicle  slip  angle  /?,  the  vehicle  yaw 
angle  ip  and  the  path  angle  <p.  Differentiating  (60)  twice  we  get 
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a  = 

R'  2  1  .  1  , 

- ~v2  H - v - M~ 

R2  R  Iz 

(83) 

where  Mz  is  the  yaw  moment  given  by 

izi>  = 

Mz  =  If/fu  —  ^rIrv 

(84) 
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Figure  29:  The  front  and  rear  wheel  friction  forces  are  uniquely  determined  in  the  yaw  stabilization 
mode. 


Consider  the  case  where  the  control  strategy  (79),  (80)  is  applied  and  condition  (81)  holds.  Also 
let  the  control  (79),  (80)  be  activated  when 


sRy  =  STX-  (85) 

Equation  (85)  is  a  necessary  condition  for  oversteer,  as  it  implies  that  the  rear  wheel  has  reached 

its  adhesion  limit.  At  this  point  we  will  have 

fRy  =  sign  (R)fR^x  (86) 

2 

TDV 

f Fy  =  --Sign  (i?)/jr,  (87) 

and  equation  (83)  can  be  written 

sign W  =  +  (88) 

The  yaw  acceleration  is  opposing  oversteer  when 

sign(i?)/3  >  0.  (89) 


Substituting  (88)  in  (89)  we  get 


v  < 


R2{tF  +  ZR)fW 


A 

=  V* 


Izsign(R)R'  +  £f\R\m 
In  summary,  we  have  that  switching  from  to  (79),  (80)  when 

SRy  <  s£ax  or  v  >  vs 

results  in  a  yawing  acceleration  that  reduces  oversteer. 


(90) 


(91) 
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3.3.6  Numerical  Example  1:  Single  Corner 

Consider  the  reference  path  of  Fig.  25.  The  trajectory  of  the  vehicle  in  Fig.  30  is  constructed  as 
follows: 

The  acceleration  phase  is  constructed  by  integration  forward  in  time  of  the  equations  of  motion 
(57)  -  (59)  starting  from  point  Pi  with  initial  conditions  (75),  (77).  The  control  switches  from 
to  (79),  (80)  whenever  the  condition  (91)  holds. 

The  deceleration  phase  is  constructed  by  integration  backwards  in  time  of  the  equations  of 
motion  (57)  -  (59)  starting  from  point  P±  with  initial  conditions  (75),  (77).  The  control  switches 
from  to  (79),  (80)  whenever  the  condition  (91)  holds. 

The  resulting  velocity  profile  is  shown  in  Fig.  31(a)  and  the  vehicle  slip  angle  j3  is  shown  in 
Fig.  31(b).  The  longitudinal,  lateral  and  total  friction  forces,  fix,  fiy  and  fi,  i  =  F,  R,  of  the  front 
and  rear  tires  are  shown  in  figures  32(a)  and  (b)  respectively. 

We  notice  that  we  achieve  a  velocity  profile  comparable  to  the  one  using  the  direct  implemen¬ 
tation  of  Section  3.3.3.  At  the  same  time  the  vehicle  slip  angle  remains  bounded  and  oversteer  is 
considerably  reduced.  Observing  the  intervals  where  the  velocity  remains  constant  (Fig.  31(a))  we 
can  identify  the  switchings  from  to  the  stabilizing  control  strategy. 


Figure  30:  Stable  implementation  of  the  optimal  control  strategy  to  the  half-car  model:  trajectory. 
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(a)  (b) 

Figure  31:  Stable  implementation  of  the  optimal  control  strategy  to  the  half-car  model:  (a)  velocity 
profile  (b)  vehicle  slip  angle. 


(a)  (b) 

Figure  32:  Stable  implementation  of  the  optimal  control  strategy  to  the  half-car  model:  front  and 
rear  tire  forces. 
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3.3.7  Numerical  Example  2:  Consecutive  Corners 

In  this  section  we  construct  the  trajectory  along  a  path  consisting  of  consecutive  corners  according 
to  the  methodology  of  Section  3.2.10,  using  the  stable  implementation  of  Section  3.3.4. 

Consider  the  path  of  Fig.  33(a).  In  Fig.  33(b)  we  can  see  the  corresponding  curvature  profile. 
Consider  fixed  initial  and  final  velocities  vq  =  Vf  =  lOm/sec  at  points  A  and  B  of  the  path 
respectively. 
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Figure  33:  (a)  A  path  with  two  consecutive  corners  (b)  Curvature  profile  of  the  path. 

According  to  the  methodology  of  Section  3.2.10  we  construct  the  following  trajectories: 

Starting  from  point  A  with  initial  yaw  states  satisfying  (75)  and  velocity  vq  we  integrate  forward 
in  time  the  equations  of  motion  (57)  -  (59).  The  control  switches  from  to  (79),  (80)  whenever 
the  condition  (91)  holds  (stable  implementation).  The  resulting  velocity  profile  I/^(s)  is  shown  in 
Fig.  34  and  the  vehicle  slip  angle  /3j[(s)  in  Fig.  35. 

Starting  from  point  B  with  initial  yaw  states  satisfying  (75)  and  velocity  vj  we  integrate  back¬ 
wards  in  time  the  equations  of  motion  (57)  -  (59).  The  control  switches  from  to  (79),  (80) 
whenever  the  condition  (91)  holds  (stable  implementation).  The  resulting  velocity  profile  Vp (s)  is 
shown  in  Fig.  34  and  the  vehicle  slip  angle  (3p(s)  in  Fig.  35. 

Starting  from  point  P\  with  initial  conditions  (75),  (77)  we  integrate  forward  in  time  the  equa¬ 
tions  of  motion  (57)  -  (59).  The  control  switches  from  to  (79),  (80)  whenever  the  condition 
(91)  holds.  The  corresponding  velocity  profile  is  (s)  and  the  vehicle  slip  angle  is  ,5^  in  figures 
34  and  35  respectively. 

Starting  from  point  Pi  with  initial  conditions  (75),  (77)  we  integrate  backwards  in  time  the 
equations  of  motion  (57)  -  (59).  The  control  switches  from  f^Q  to  (79),  (80)  whenever  the  condition 
(91)  holds.  The  corresponding  velocity  profile  is  V^(s)  and  the  vehicle  slip  angle  is  (3p  in  figures 
34  and  35  respectively. 

Finally  we  construct  the  velocity  and  vehicle  slip  angle  profiles  for  P2  Vpo  and  f3p2  in  figures  34 
and  35  respectively,  similar  to  the  characteristics  corresponding  to  point  Pi. 

In  the  point  mass  case  (Section  3.2.10)  the  switching  from  one  trajectory  to  the  other  is  de¬ 
termined  by  the  intersection  points  of  the  corresponding  velocity  profiles.  In  the  half-car  model, 
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Figure  34:  Stable  implementation  to  consecutive  corners:  velocity  profile. 


where  the  state  vector  is  extended  by  the  yaw  dynamics  states,  however  the  intersection  points  of 
the  velocity  profiles  do  not  necessarily  coincide  with  the  intersection  points  of  the  vehicle  slip  angle. 
For  instance  V^"(s)  and  V^(s)  in  Fig.  34  intersect  at  s  =  5.95m  (point  SPai),  while  P\{s)  and 
Pp  (s)  intersect  at  s  =  6.3m. 

On  the  other  hand,  using  the  stable  implementation  of  Section  3.3.4  to  construct  the  trajectories 
along  each  sub-arc  results  in  trajectories  with  bounded  values  of  the  yaw  states.  In  other  words 
the  vehicle  slip  angle  profiles  are  considerably  close  in  the  area  of  intersection  of  the  corresponding 
velocity  profiles.  The  switching  points  for  the  overall  trajectory  are  chosen  by  trial  and  error,  in 
the  area  of  intersection  of  the  velocity  profiles  aiming  to  generate  a  velocity  profile 

K(s)^mm{F+(S),Fp^  (92) 

The  velocity  profile  V0(s)  along  the  whole  path  is  shown  in  Fig.  34  and  the  corresponding  vehicle 
slip  angle  /30(s)  is  shown  in  Fig.  35.  The  trajectory  of  the  vehicle  along  the  path  is  shown  in  Fig.  36. 

The  first  switching  from  acceleration  to  deceleration  is  adjusted  from  SPai  to  the 
point  ASPai-  Switching  control  exactly  at  SPai  results  in  a  solution  Ve (s)  that  diverges  from  (92). 
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Figure  35:  Stable  implementation  to  consecutive  corners:  vehicle  slip  angle. 


Figure  36:  Stable  implementation  to  consecutive  corners:  trajectory. 
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3.3.8  “Zero-Slip”  Implementation 


In  this  section  we  present  a  different  extension  of  the  methodology  of  Section  3.2.10  to  the  half-car 
model.  This  time  we  impose  the  additional  constraint  that  the  vehicle  tracks  the  path  with  zero 
slip  angle  in  order  to  completely  eliminate  the  need  for  yaw  stability  considerations. 

For  exact  tracking  of  the  curvature  profile  k(s)  with  vehicle  slip  angle  (3  =  0  the  centripetal 
force  on  the  vehicle  is  given  by 


fn  =  =  f Fy  +  fRy  (93) 

For  (3  =  0  we  have  that  the  path  angle  f>  coincides  with  the  yaw  angle  ip  of  the  vehicle.  That  is  for 
(3  =  0 


=  cp(s),  ip'(s)  =  4/{s)  =  n(s),  ip"{s)  =  (p'\s)  =  k'(s).  (94) 

Notice  that 

ip  =  ip"v2  +  ip'v'v.  (95) 

The  yaw  dynamics  (59)  can  then  be  written  as 

Iz1p"v2  +  Iz1p'v'v  =  lFfFy  -  £ RfRy ,  (96) 


or  using  (93) 


Iz^"v2  +  Lip'v'v  =  (Fnmv2  -  {£F  +  £ R)fRy- 
Consider  now  the  longitudinal  dynamics 

mv  =  ft  =>•  mvv'  =  ft- 


(97) 


(98) 


Equation  (97)  can  now  be  written  as 


ft 


Izkvz  +  Izn—  =  £fk  mvz 
m 

m(£FKmv 2  —  Izk'v2) 

ft  =  - j - 

Izn 


(£f  +  £n)fRy  => 

m(lF  +  Ir) 

t  fRy 


(99) 


Equation  (99)  above  provides  the  necessary  tangential  force  ft  for  the  vehicle  to  track  k(s)  with 
(3  =  0,  given  the  rear  lateral  force  fRy.  The  linear  map  ft(fRy)  of  (99)  is  shown  in  Fig.  37. 

The  rear  tire  lateral  slip  with  (3  =  0  is  given  by 


SRy 


V 


Ip' v£r 

V 


-k£r, 


(100) 


and  the  rear  lateral  force  changes  with  srx  G  [— 1,  +1]  as  in  Fig.  21.  Accordingly  we  can  find  the 
range  of  ft  using  (99). 

For  a  given  operating  condition  of  the  vehicle  v,  ip,  {(3  =  0),  i.e.  for  a  given  value  of  srv,  each 
value  of  srx  G  [— 1,+1]  corresponds  to  unique  values  of  Prx  and  pRy  from  (61)  and  a  unique  value 
of  ft  from  (99).  The  front  tire  longitudinal  force  fFx  is  given  by 


fFx(fRxi  fRy)  —  ft(fRy)  fRx- 


(101) 
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Figure  37:  Forces  diagram  for  tracking  with  j3  =  0. 


The  front  longitudinal  forces  fFxifRx,  fRy )  from  (101),  for  srx  G  [— 1,+1]  are  shown  in  Fig.  37. 

Finally,  in  order  for  the  front  tire  friction  force  to  remain  in  the  front  tire  friction  circle  we  have 
to  ensure  that 


\fFx\  <  y/( frx)2-f2Fy  =  \/(/rx)2  -  (fRy  -  Kmv’iy  ±  ff™.  (102) 

The  bounds  from  (102)  are  shown  in  Fig.  37. 

An  extension  of  the  methodology  of  Section  3.2.10  to  a  half-car  model  tracking  a  prescribed 
path  k(s)  with  vehicle  slip  angle  (3  =  0  becomes  evident. 

The  maximum  available  accelerating  front  longitudinal  force  fpx  is  given  by  the  intersection  of 
fpx  from  (101)  with  fpf*  from  (102).  This  defines  f  as  in  Fig.  37  which  provides  the  maximum 
available  accelerating  force  of  the  vehicle  /j*+.  The  control  /*+  corresponds  to  the  maximum 
acceleration  u  =  +1  for  the  point  mass  model  of  Section  3.2.10.  If  there  is  no  intersection  of  fpx 
from  (101)  with  fjffx  from  (102)  the  maximum  available  acceleration  /f*+  is  given  for  srx  =  —1  as 
in  Fig.  38(a). 

Equivalently,  the  maximum  available  decelerating  front  longitudinal  force  fpx  is  given  by  the 
intersection  of  fpx  from  (101)  with  — /™^x  from  (102).  This  defines  and  _/)*“.  The  control  /j*- 
corresponds  to  the  maximum  deceleration  u  =  —  1  for  the  point  mass  model  of  Section  3.2.10.  If 
there  is  no  intersection  of  fpx  from  (101)  with  —  from  (102)  the  maximum  available  deceleration 
fl~  is  given  for  srx  =  0  as  in  Fig.  38(b). 

As  velocity  decreases,  ft  for  srx  =  ±1  becomes  negative,  which  implies  a  negative  maximum 
accelerating  force  /4*+  <  0  as  in  Fig.  39(a).  Equivalently,  as  velocity  increases,  ft  for  srx  =  0 
becomes  negative,  which  implies  a  positive  maximum  decelerating  force  >  0  as  in  Fig.  39(b). 
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Figure  38:  (a)  f*+  for  sRy  =  -1.  (b)  ff  for  sRy  =  0. 

In  order  to  avoid  such  problematic  cases  we  switch  from  to  the  following  control 

ft  =  0,  when  >  0  or  ff  +  <  0.  (103) 


Figure  39:  (a)  Negative  maximum  acceleration  ff  +  (b)  Positive  maximum  deceleration  /f* 


3.3.9  Numerical  Example:  Zero-Slip  Implementation 

Consider  the  path  of  Fig.  33.  This  time  we  calculate  the  trajectory  of  the  vehicle  using  the  zero-slip 
strategy  of  Section  3.3.8. 

First  we  need  to  derive  the  optimal  states  at  the  points  of  minimum  magnitude  of  radius  P\ 
and  P2 .  As  for  the  whole  trajectory  we  take  /3(sPl)  =  /3(sp2 )  =  0.  From  equations  (94)  we  get 

ip(spi)  =  ilj'(spi)v(spi)  =  K(sPi)v(sPi),  *  =  1,2.  (104) 

To  maximize  the  centripetal  force  at  P\  and  P2  we  enforce  sRx(sPi )  =  0,  i  =  1,2  to  maximize 
fRy  which  lies  along  the  normal  direction  n.  The  rear  lateral  force  is  calculated  from  (61),  given 
sRy  =  —n£r.  The  initial  velocity  v(sPi),  i  =  1,2  is  calculated  then  from  (99)  setting  ft  =  0. 
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The  optimization  scheme  of  Section  3.3.7  is  repeated  here  using  /t*+  for  maximum  acceleration 
and  f*~  for  maximum  deceleration.  The  resulting  velocity  profile  (Fig.  40)  reveals  that  the  “zero- 
slip”  implementation  of  Section  3.3.8  is  conservative  compared  to  the  “stable”  implementation  of 
Section  3.3.7.  Stability  and  continuity  of  the  yaw  states  is  however  a-priori  guaranteed  in  the 
“zero-slip”  strategy.  Figure  41  shows  the  calculated  trajectory. 


Figure  40:  Zero-slip  implementation  to  consecutive  corners:  velocity  profile. 


3.4  Conclusions  and  Future  Work 

In  this  work  we  presented  semi-analytic  methodologies  to  generate  optimal  and  near-optimal  min¬ 
imum  time  velocity  profiles  for  ground  vehicles  along  a  prescribed  path. 

First,  we  presented  the  point  mass  model  case  and  a  constructive  proof  of  optimality  of  the 
minimum  time  solution.  The  methodology  accounts  for  the  loss  of  controllability  due  to  the  coupling 
of  accelerating/braking  with  centripetal  forces.  In  addition,  the  necessary  optimality  conditions 
were  derived,  which  provide  an  estimate  of  the  number  and  type  of  control  switchings  according 
to  the  geometry  of  the  prescribed  path.  A  receding  horizon  scheme  has  also  been  proposed  to 
lower  the  computational  cost  of  implementing  the  proposed  analytic  approach,  and  to  account  for 
unpredictable  changes  in  the  environment.  Numerical  simulations  show  that  the  proposed  on-line 
velocity  optimizer  is  competitive  when  compared  to  lap  times  obtained  by  expert  FI  race  drivers. 

Next,  we  presented  several  extensions  of  the  point  mass  case  methodology  to  a  vehicle  model 
that  includes  the  yaw  dynamics.  We  introduced  a  half-car  model  and  calculated  the  acceleration 
enveloped  from  the  tire  friction  forces  on  the  front  and  rear  axles.  Direct  implementation  of  the 
point  mass  control  strategy  to  the  half-car  model  case  revealed  the  need  to  design  control  schemes 
taking  yaw  stability  into  consideration.  We  followed  two  different  approaches  towards  a  stable  with 
respect  to  yaw  dynamics  implementation.  In  the  first  approach  we  designed  a  control  scheme  that 
intervene  during  execution  of  the  optimal  maximum  acceleration/maximum  deceleration  action 
when  the  vehicle  oversteers  and  the  yaw  dynamics  tend  to  instability.  In  the  second  approach  we 
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Figure  41:  Zero-slip  implementation  to  consecutive  corners:  trajectory. 


redefined  the  maximum  acceleration  limits  of  the  vehicle  subject  to  the  additional  constraint  of 
zero  vehicle  slip  angle  throughout  the  path. 

The  zero-slip  implementation  of  the  point-mass  methodology  to  the  half-car  completely  elim¬ 
inates  the  problems  associated  with  yaw  stability.  Preliminary  results  presented  above,  however, 
show  that  the  zero-slip  implementation  generates  conservative  results.  To  this  end  we  will  continue 
towards  developing  a  stable  implementation  scheme  according  to  Section  3.3.4  that  will  compensate 
for  both  oversteer  and  understeer  of  the  vehicle. 

Ultimately  we  plan  to  develop  a  hybrid  numerical/semi-analytic  optimization  scheme  to  cal¬ 
culate  optimal  trajectories  for  ground  vehicles.  The  semi-analytic  methodologies  presented  in  this 
work  can  be  used  to  provide  the  necessary  initial  guesses  to  numerical  optimization  schemes,  which 
allow  for  further  increase  in  the  fidelity  of  the  vehicle  models  used  and  generate  realistic  results. 
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